%ФИЛЬТР КАЛМАНА
dV_Filtered = zeros (N,1);%Отфильтрованные измерения (Оценки ошибок по скорости)
Fi_Filtered = zeros (N,1);%Отфильтрованные измерения (Оценки ошибок по углу невыставки)
dW_Filtered = zeros (N,1);%Отфильтрованные измерения (Оценки ошибок по дрейу)

%МАТРИЦЫ
Q = 1E-14;                %Матрица ковариации входного шума принятая в фильтре
R = 0.1;                  %Матрица ковариации измерительного шума принятая в фильтре  
%НАЧАЛЬНАЕ ЗНАЧЕНИЯ
est_X0 = [0;0;0];%Начальное значение оценки ветора состояния
est_X_prev = est_X0;                    
P0 = [1 0 0;     %Корреляционная матрица
      0 1 0;
      0 0 1];
P_prev = P0;
Nuk = [0; 0; 0];

%УРАВНЕНИЯ РИКАТТИ
prcnt10 = N/100;
p_count = 0;
for i = 1:1:N
    est_X_trans = F*est_X_prev;
    Zk = dV_measured(i,1);
    Nuk = Zk - H*F*est_X_prev;
    P_trans = F*P_prev*(F') + G*Q*(G.');
    Kk = P_trans*(H.')/(H*P_trans*(H.') + R); 
    est_X_next = est_X_trans + Kk*Nuk;
    P_next = (I - Kk*H)*P_trans;
    
    dV_Filtered(i,1) = est_X_prev(1,1);
    Fi_Filtered(i,1) = est_X_prev(2,1);
    dW_Filtered(i,1) = est_X_prev(3,1);
    
    est_X_prev = est_X_next;
    P_prev = P_next;
    if (mod(i,prcnt10) == 0)  
       clc;
       fprintf('Filtering complete %d%%', p_count + 1);
       p_count = p_count + 1;
    end    
end;
% subplot(5,1,5)
% plot(time',dV_Filtered(:,1))
% title('delta V Estimated')
% ylabel('m/s')

%СРАВНЕНИЕ МОДЕЛИ, ИЗМЕРЕНИЙ И ОЦЕНОК
figure
plot1 = plot(time', dV(:,1), 'blue', time', dV_measured(:,1),'green', time', dV_Filtered(:,1), 'red');               
title('delta V - blue, delta V measured - green, delta V estimated - red');
xlabel('sec');
ylabel('m/s');
exclude_num = 200;
figure
plot2 = plot(time(exclude_num:N)', Fi(exclude_num:N,1), 'blue', time(exclude_num:N)', Fi_Filtered(exclude_num:N,1), 'red');                       
title(' Fi - blue, Fi estimated - red');
xlabel('sec');
ylabel('rad');
exclude_num = 450;
figure
plot3 = plot(time(exclude_num:N)', dW(exclude_num:N,1), 'blue', time(exclude_num:N)', dW_Filtered(exclude_num:N,1), 'red');           
title(' dW - blue, dW estimated - red');
xlabel('sec');
ylabel('rad');