% ИСХОДНЫЕ ДАННЫЕ
%Постоянные величины
n = 5000;                              %Время интегрирования (с)
N = n;                                   %Число шагов интегрирования   
Rz =6371000;                             %Радиус Земли (м)
g = 9.8065;                              %Ускорение свободного падения(м/с^2)
%Параметры системы
T = 1E0;                                   %Величина шага дискретизации (с)
beta = 5E-8;                             %Дрейфа (с)

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


H = [1 0 0];                                      %Матрица наблюдаемости 
G = [0; 0; 1];                                   %Весовая матрица входных шумов 
Zk = [0 0 1];                                %Матрица измерений
Xk = [0;0;0];                                 %Вектор состояния 
Xk_next = [0;0;0];                                  %Вектор состояния на следующем шаге
dX = [0;0;0];
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 + A^3.*T^3/6 + A^4.*T^4/24 + A^5.*T^5/120 + A^6.*T^6/720 + A^7.*T^7/5040;  %Матрица перехода 

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


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