clc
clear
format short e
%******************************************************************
Rad_Degree=1/180.0*pi; % “度”轉為“弧度”
zh=1/180*pi/3600;% “度/小時”轉換成“弧度/秒”
A=zeros(17);
%*********************************
Vx=8.0;
Vy=8.0;
Vz=0.0;
g=9.8;
angle=45*pi/180;
R=6371020.0;
Wie=7.2921158*10^(-5);
fx=(2*Wie*cos(angle)+Vx/R)*Vz-(2*Wie*sin(angle)+Vx*tan(angle)/R)*Vy;
fy=(2*Wie*sin(angle)+Vx*tan(angle)/R)*Vx+Vy*Vz/R;
fz=-(2*Wie*cos(angle)+Vx/R)*Vx-Vy^2/R+g;
A(1,2)= Vx*tan(angle)/R+Wie*sin(angle);
A(1,3)=-Vx/R-Wie*cos(angle);
A(1,8)=-1/R;
A(1,10)=-1;
A(2,1)=-Vx*tan(angle)/R-Wie*sin(angle);
A(2,3)=-Vy/R;
A(2,7)=1/R;
A(2,9)=-Wie*sin(angle);
A(2,11)=-1;
A(3,1)=Vx/R+Wie*cos(angle);
A(3,2)=Vy/R;
A(3,7)=tan(angle)/R;
A(3,9)=Wie*cos(angle)+Vx*(sec(angle))^2/R;
A(3,12)=-1;
A(4,10)=1;
A(4,13)=1;
A(5,11)=1;
A(5,14)=1;
A(6,12)=1;
A(6,15)=1;
A(7,2)=-fz;
A(7,3)=fy; 
A(7,7)=(Vy*tan(angle)-Vz)/R;
A(7,8)=2*Wie*sin(angle)+Vx*tan(angle)/R; 
A(7,9)=2*Wie*sin(angle)*Vz+(2*Wie*cos(angle)+Vx*(sec(angle))^2/R)*Vy;
A(7,16)=1;
A(8,1)=fz;
A(8,3)=-fx;
A(8,7)=-2*Wie*sin(angle)-2*Vx*tan(angle)/R;
A(8,8)=-Vz/R;
A(8,9)=-2*Wie*cos(angle)*Vx-Vx^2*(sec(angle))^2/R;
A(8,17)=1;
A(9,8)=1/R;
%****************************************
%***************************************
c=zeros(5,17);
c(1,7)=1;
c(2,8)=1;
c(3,1)=-1;
c(3,4)=1;
c(4,2)=-1;
c(4,5)=1;
c(5,3)=-1;
c(5,6)=1;
%*****************************************
%aa=[a(1,:);a(2,:);a(3,:);a(4,:);a(5,:);a(9,:);a(10,:)];
%A=[aa(:,1) aa(:,2) aa(:,3) aa(:,4) aa(:,5) aa(:,9) aa(:,10)];
%*****************************************
B=[eye(17) zeros(17,5)];
%*********************************************
C=c;
%********************************************
gama=0.3;
L=zeros(2,17);
L(1,1)=0.001;
L(2,2)=0.001;
%******************************************************************
D=[zeros(5,17) eye(5)];
%--------------solve riccati equation-------------
AA=A';
BB=[L' C'];
Q=B*B';
m1=size(L',2);
m2=size(C',2);
R=[-gama^2*eye(m1) zeros(m1,m2);zeros(m2,m1) eye(m2)];
P=care(AA,BB,Q,R);
K=P*C';
%---------------------------當w和v是白噪聲時--------------------------------------
x=[3*pi/(60*180),3*pi/(60*180),5*pi/(60*180),3*pi/(60*180),3*pi/(60*180),5*pi/(60*180),0.1,0.1,2/60*pi/180,0.1*pi/180/3600,0.1*pi/180/3600,0.1*pi/180/3600,0,0,0,1e-4*g,1e-4*g]';
gjx=[3*pi/(60*180),3*pi/(60*180),5*pi/(60*180),3*pi/(60*180),3*pi/(60*180),5*pi/(60*180),0.1,0.1,2/60*pi/180,0.1*pi/180/3600,0.1*pi/180/3600,0.1*pi/180/3600,0,0,0,1e-4*g,1e-4*g]';
for i=0:1:10000
   step=0.01;
   snoise=[0.01*pi/180/3600*randn(1) 0.01*pi/180/3600*randn(1) 0.01*pi/180/3600*randn(1) 0.01*pi/180/3600*randn(1) 0.01*pi/180/3600*randn(1) 0.01*pi/180/3600*randn(1) 1e-5*g*randn(1) 1e-5*g*randn(1) 0 0 0 0 0 0 0 0 0]';
%量測噪聲
mnoise=[0.1*randn(1) 0.1*randn(1) 1/60*pi/180*randn(1) 1/60*pi/180*randn(1) 3/60*pi/180*randn(1)]';
   w=[0.0005*randn(2,1);0.01*zh*randn(3,1);0;0;0.001*randn(5,1)];
   dx=x;
   k1=A*x+B*w;   
   x1=dx+1/2*step*k1;
   k2=A*x1+B*w;
   x2=dx+1/2*step*k2;
   k3=A*x2+B*w;
   x3=dx+step*k3;
   k4=A*x3+B*w;
   k=1/6*(k1+2*k2+2*k3+k4);    
   x=x+step*k;
   y=C*x+D*w;    
   z=L*x; 
   realx3(i+1)=x(6);
   realx4(i+1)=x(7);
   realx5(i+1)=x(5);   
   step=0.01;
   PP=A-K*C;
   [FF,GG]=c2d(PP,K,step);
   gjx=FF*gjx+GG*y;
   estimatex3(i+1)=gjx(6);
   estimatex4(i+1)=gjx(7);
   estimatex5(i+1)=gjx(5);
  end
%畫圖
i=0:0.01:100;
figure(1);
subplot(3,1,1);
plot(i,realx3/zh,'b:',i,estimatex3/zh,'r:');
grid;
ylabel('東向水平失準角');
subplot(3,1,2);
plot(i,realx4/zh,'b:',i,estimatex4/zh,'r:');
grid;
ylabel('北向水平失準角');
subplot(3,1,3);
plot(i,realx5,'b:',i,estimatex5,'r:');
grid;
xlabel('time(s)');
ylabel('方位失準角');