%АДАПТИВНЫЙ ФИЛЬТР КАЛМАНА ПЕРВОГО РОДА

%МАТРИЦЫ
Q = 1E-14;                %Матрица ковариации входного шума принятая в фильтре
%НАЧАЛЬНАЕ ЗНАЧЕНИЯ
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];
sigma_Nuk = 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');            %Априорная ошибка оценивания
    sigma_Nuk = sigma_Nuk + Nuk*Nuk';
    est_Ck = 1/i*sigma_Nuk;                         %Осредненное за время измерения значение матрицы Ck
    
%     est_Ck = Nuk * Nuk';                            %Оценка матрицы Ck
    est_R = est_Ck - H*P_trans*H';
    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;