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=3;
L=zeros(3,17);
L(1,1)=0.001;
L(2,2)=0.001;
L(3,3)=0.001;
%******************************************************************
A1=A;
%******************************************************************
A2=A;
%******************************************************************
D=[zeros(5,17) eye(5)];
%*******************************************************************
%*******************************************************************
setlmis([])
X=lmivar(1,[17 1])
R=lmivar(1,[17 1])
M=lmivar(2,[17 17])
Z=lmivar(2,[17 5])
%******************************************************************
%******************************************************************
lmiterm([1 1 1 R],1,A1,'s')
lmiterm([1 2 1 R],A1',1)
lmiterm([1 2 1 X],1,A1)
lmiterm([1 2 1 Z],1,C)
lmiterm([1 2 1 M],1,1)
lmiterm([1 1 3 R],1,B)
lmiterm([1 4 1 0],L)
lmiterm([1 4 1 0],0)
lmiterm([1 4 1 0],-L)
lmiterm([1 2 2 X],1,A1,'s')
lmiterm([1 2 2 Z],1,C,'s')
lmiterm([1 2 3 X],1,B)
lmiterm([1 2 3 Z],1,D)
lmiterm([1 4 2 0],L)
lmiterm([1 4 2 0],0)
lmiterm([1 3 3 0],-1)
lmiterm([1 4 3 0],0)
lmiterm([1 4 4 0],-gama^2)
%******************************************************************
lmiterm([2 1 1 R],1,A2,'s')
lmiterm([2 2 1 R],A2',1)
lmiterm([2 2 1 X],1,A2)
lmiterm([2 2 1 Z],1,C)
lmiterm([2 2 1 M],1,1)
lmiterm([2 1 3 R],1,B)
lmiterm([2 4 1 0],L)
lmiterm([2 4 1 0],0)
lmiterm([2 4 1 0],-L)
lmiterm([2 2 2 X],1,A2,'s')
lmiterm([2 2 2 Z],1,C,'s')
lmiterm([2 2 3 X],1,B)
lmiterm([2 2 3 Z],1,D)
lmiterm([2 4 2 0],L)
lmiterm([2 4 2 0],0)
lmiterm([2 3 3 0],-1)
lmiterm([2 4 3 0],0)
lmiterm([2 4 4 0],-gama^2)
%********************************************************************
lmiterm([-3 1 1 R],-1,1)
lmiterm([-3 1 1 X],1,1)
lmiterm([3 1 1 0],1e-8)
%********************************************************************
lmiterm([-4 1 1 R],1,1)
%********************************************************************
lmis=getlmis
decinfo(lmis,X)
decinfo(lmis,R)
decinfo(lmis,M)
decinfo(lmis,Z)
%********************************************************************
[tmin,xfeas]=feasp(lmis,[0 0 10 0 0],-1)
%*********************************************************************
X=dec2mat(lmis,xfeas,X);
%**********************************************************************
R=dec2mat(lmis,xfeas,R);
%**********************************************************************
M=dec2mat(lmis,xfeas,M);
%***********************************************************************
%***********************************************************************
Z=dec2mat(lmis,xfeas,Z);
%***********************************************************************
%*************************************************************************
%*************************************************************************
Af=inv(R-X)*M;
Bf=inv(R-X)*Z;
%*************************************************************************
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:252:500
   step=2;
   w=[0.01*zh*randn(6,1);1e-5*g*randn(2,1);zeros(9,1);0.1*randn(2,1);1/60*pi/180*randn(2,1);3/60*pi/180*randn(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;    
   realx3(i+1)=x(3);
   realx4(i+1)=x(4);
   realx5(i+1)=x(5);   
   step=2;
   [FF,GG]=c2d(Af,Bf,step);
   gjx=FF*gjx+GG*y;
   estimatex3(i+1)=gjx(3);
   estimatex4(i+1)=gjx(4);
   estimatex5(i+1)=gjx(5);
  end
%畫圖
i=0:0.01:100;
figure(1);
subplot(3,1,1);
plot(i,realx3/Rad_Degree*60,'b:',i,estimatex3/Rad_Degree*60,'r:');
grid;
ylabel('東向水平失準角');
subplot(3,1,2);
plot(i,realx4/Rad_Degree*60,'b:',i,estimatex4/Rad_Degree*60,'r:');
grid;
ylabel('北向水平失準角');
subplot(3,1,3);
plot(i,realx5/Rad_Degree*60,'b:',i,estimatex5/Rad_Degree*60,'r:');
grid;
xlabel('time(s)');
ylabel('方位失準角');