%%Altitude with inertial and GPS measurement
%%JLJ 2014
clear all;
close();
times=20; %%total simulation time
T=1; %%s time between measures
ts=1/10 %%discrete system sample time
samplesno=times/ts;
Pz0=0; %%Initial values
Pv0=2.5e-5;
Pb0=1e-6;
R=4; 
pxminus=[Pz0 0 0;0 Pv0 0;0 0 Pb0]
Q=pxminus;
%%Q=[0 0 0;0 Pv0 0;0 0 Pb0];
%%L=[0.3,0.039,0.002]' 
L=[0.4,0.04,0.002]'     
A=[0 1 0;0 0 1;0 0 0]
C=[1,0,0]
D=[0 0]
bigtheta=[1 1 .5; 0 1 1;0 0 1]

%%Altitude variance
Pz(1)=pxminus(1,1)
for i=1:samplesno
    Qd=Q*T+(A*Q+Q*A')*T^2/2+(A^2*Q+2*A*Q*A'+Q*(A')^2)*T^3/6;
    t(i)=i*T;
    pxplus=(eye(3,3)-L*C)*pxminus*(eye(3,3)-L*C)+L*R*L'
    pxminuskuno=bigtheta*pxplus*bigtheta'+Qd
    Pz(i)=pxminus(1,1)
    pxminus=pxminuskuno;
    Pz(i+1)=pxminus(1,1)
end
figure(1);
plot(t,(Pz(1,1:samplesno)).^0.5,'+')
title('Position error deviation Vs Sample number','fontsize',18)
xlabel('$k,t=kT$','Interpreter','LaTex','fontsize',15)
ylabel('$\sqrt{p_z(kT)}[m]$','Interpreter','LaTex','fontsize',15)
grid

%%Realization details
Ains=[0 1 0;0 0 1;0 0 0]
Bins=[0;1;0]
Cins=[1 0 0]
Dins=[0]
INS=ss(Ains,Bins,Cins,Dins)
u=0.5*ones(1,times)
ud=0.5*ones(1,samplesno)
tc=linspace(0,times,times/T)
td=linspace(0,times,samplesno)
[yins tins xins]=lsim(INS,u,tc)
%%Discrete time numerical approach
INS_discrete=c2d(INS,ts,'tustin')
[yins_discrete tins_discrete xins_discrete]=lsim(INS,u,tc)

%%Analytical 'zoh' 
Aana=[1 ts 0;0 1 ts;0 0 0]
Bana=[0;ts;0]
Cana=[1 0 1]
Dana=[0]
INS_ana=ss(Aana,Bana,Cana,Dana,ts)
[yana tana xama]=lsim(INS_ana,ud)
figure(2)
plot(tins,u,'black',tins,yins,'ob',tins_discrete,yins_discrete,'--r',td,yana,'+g')
title('INS model input Vs output different implementations','fontsize',16)
xlabel('Time [s] and sample number','fontsize',14)
ylabel('Input yellow, Position [m]','fontsize',14)
legend('Input','Continuous','Discrete Tustin','Discrete zoh');

