%ОБРАТНЫЙ ФИЛЬТР КАЛМАНА
Backward_dV_Filtered = zeros (N,1);%Отфильтрованные измерения (Оценки ошибок по скорости)
Backward_Fi_Filtered = zeros (N,1);%Отфильтрованные измерения (Оценки ошибок по углу невыставки)
Backward_dW_Filtered = zeros (N,1);%Отфильтрованные измерения (Оценки ошибок по дрейу)
%НАЧАЛЬНАЕ ЗНАЧЕНИЯ
est_X0 = [dV_measured(N,1);Fi_Filtered(N,1);dW_Filtered(N,1)];%Начальное значение оценки ветора состояния
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 = N:-1:1
    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;
    
    Backward_dV_Filtered(i,1) = est_X_prev(1,1);
    Backward_Fi_Filtered(i,1) = est_X_prev(2,1);
    Backward_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('Backward Filtering complete %d%%', p_count + 1);
       p_count = p_count + 1;
    end    
end;
P_k_B = P_next;
% 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', Backward_dV_Filtered(:,1), 'red');               
% title('Backward delta V - blue, Backward delta V measured - green, Backward delta V estimated - red');
% xlabel('sec');
% ylabel('m/s');
% exclude_num = 200;
% figure
% plot2 = plot(time(1:N-exclude_num)', Fi(1:N-exclude_num,1), 'blue', time(1:N-exclude_num)', Backward_Fi_Filtered(1:N-exclude_num,1), 'red');                       
% title(' Backward Fi - blue, Backward Fi estimated - red');
% xlabel('sec');
% ylabel('rad');
% exclude_num = 450;
% figure
% plot3 = plot(time(1:N-exclude_num)', dW(1:N-exclude_num,1), 'blue', time(1:N-exclude_num)', Backward_dW_Filtered(1:N-exclude_num,1), 'red');           
% title(' Backward dW - blue, Backward dW estimated - red');
% xlabel('sec');
% ylabel('rad');