clear all
clc
% ИСХОДНЫЕ ДАННЫЕ

%Параметры системы
T = 1e-1;                                %Величина шага дискретизации (с)
beta = 5E-8;                             %Дрейфа (с)
%Постоянные величины
t = 10000;                               %Время интегрирования (с)
N = t/T;                                 %Число шагов интегрирования   
Rz =6371000;                             %Радиус Земли (м)
g = 9.80665;                             %Ускорение свободного падения(м/с^2)

%МАТРИЦЫ
R = 0.01;                                      %Матрица ковариации измерительного шума модели
Q = 1E-14;                                     %Матрица ковариации входного шума модели
X0 = [0 ; 0 ; 0];                              %Вектор начальных условий

H = [1 0 0]                                      %Матрица наблюдаемости                                       
G = [0 0 0;                                      %Весовая матрица входных шумов
     0 0 0;
     0 0 1];                                    
Zk = 0;                                          %Матрица измерений
Xk = X0;                                         %Вектор состояния 
I = eye(3);
A = [0,       g,     0; 
    -1/Rz,    0,     1; 
     0,       0,    -beta];
% Wk = sqrt(Q).*randn;                            %Вектор входных шумов 

F =I + A.*T + A^2.*T^2/2;  %Матрица перехода 

% Uk = sqrt(R)*randn;                      %Вектор измеритльных шумов                                   


%Задание начальных условий
% Xk = X0;
%Инициализация выходных массивов
dV = zeros(N,1) ;                                
Fi = zeros(N,1) ;
dW = zeros(N,1) ;
dV_measured = zeros(N,1);
prcnt10 = N/100;
p_count = 0;
%РАСЧЕТ МОДЕЛИ
for i = 1:1:N
    
    Xk = F*Xk + [0;0;sqrt(Q)*randn].*T;     %Уравнение в матрично-векторной форме   
    Zk = H*Xk + sqrt(R).*randn;             %Вектор измерений
                       
   %Заполнение выходных массивов 
    dV(i,1) = Xk(1,1);
    dV_measured(i,1) = Zk;
    Fi(i,1) = Xk(2,1);
    dW(i,1) = Xk(3,1);
    if (mod(i,prcnt10) == 0)  
       clc;
       fprintf('Modeling complete %d%%', p_count + 1);
       p_count = p_count + 1;
    end    
end;
time = 0:T:t-T;
% subplot(4,1,1)
% plot(time', dV(:,1))
% title('delta V');
% ylabel('m/s');
% xlabel('sec');
% 
% subplot(4,1,2)
% plot(time', Fi(:,1))
% title('delta Fi');
% ylabel('rad');
% xlabel('sec');
% 
% subplot(4,1,3)
% plot(time', dW(:,1))
% title('delta Epsilon');
% ylabel('rad');
% xlabel('sec');
% 
% subplot(4,1,4)
% plot(time', dV_measured(:,1))
% title('delta V measured');
% ylabel('m/s');
% xlabel('sec');

%ЗАПУСК ФИЛЬТРАЦИИ
Kalman_1F
Kalman_1B

%СРАВНЕНИЕ ПРЯМОГО И ОБРАТНОГО ФИЛЬТРОВ, А ТАКЖЕ ИЗМЕРЕНИЙ
figure
 plot1 = plot(time', dW(:,1),'green', time', Backward_dW_Filtered(:,1), 'red',time', dW_Filtered(:,1), 'blue');               
 title('delta V - green, delta V estimated - blue, Backward delta V estimated - red');
 xlabel('sec');
 ylabel('m/s');