//Altitude with inertial and GPS measurement
//BAD JLJ 2014
clear;
close();
times=10; //total simulation time
T=1; //s time between measures
ts=1/100 //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)
axes=gca();
axes.font_size=4;
x_label=axes.x_label;
y_label=axes.y_label
x_label.font_size=4;
y_label.font_size=4;
title('Position error deviation Vs Sample number','fontsize',4)
xlabel('$k,t=kT$')
ylabel('$\sqrt{p_z(kT)}[m]$')
plot(t,(Pz(1:samplesno,1))^0.5,'+')
xgrid
//Realization details
Ains=[0 1 0;0 0 1;0 0 0]
Bins=[0;1;0]
Cins=[1 0 0]
Dins=[0]
INS=syslin('c',Ains,Bins,Cins,Dins)
//Brute force
INS_discrete=cls2dls(INS,ts)
//Analytical
Ainsd=[1 ts 0;0 1 ts;0 0 1]
Binsd=[0;ts;0]
INS_ana=syslin('d',Ainsd,Binsd,[1 0 0],[0])
u=0.5*ones(1,times)
ud=0.5*ones(1,samplesno)
tc=linspace(0,times,times/T)
td=linspace(0,times,samplesno)
[yins xins]=csim(u,tc,INS)
//Numerical 
[xins_discrete]=ltitr(INS_discrete.A,INS_discrete.B,ud)
//Analytical
[xana]=ltitr(Ainsd,Binsd,ud)

//[xins_ana]=ltitr(Ainsd,Binsd,ud)

//Continuous blue vs discrete vs Analytical discrete
figure(2);
axes=gca();
axes.font_size=4;
x_label=axes.x_label;
y_label=axes.y_label
x_label.font_size=4;
y_label.font_size=4;
title('INS altitude outupt Vs Input, different implementations' ,'fontsize',4)

xlabel("Time [s]")
ylabel("Input, Altitude [m]")
plot(tc,u,'black',tc,yins,'oblue',td,xins_discrete(1,:),'green',td,xana(1,:),'--r')
legend('Input','Continuous','Numerical Tustin Discrete','Analytical zoh')

//figure(3)
