wd=45.6679;
wie=7.2921158e-5;
R=6378.393*1000;
g=9.78049;

t=0.05;
wiev=wie*sin(wd*pi/180.0);
wien=wie*cos(wd*pi/180.0);

wiet=[0,wien,wiev]';
wb=[0,wien,wiev]';
ab=[0.01,0.01,g]';

X=[0.0,-0.0,0.00,0,0]';
temp=zeros(5);
r=4990;
A=[0,wiev,-wien,0,-1/R;
   -wiev,0,0,1/R,0;
   wien,0,0,(wiev/wien)/R,0;
    0,-g,0,0,2*wiev;
   g,0,0,-2*wiev,0]
G=[0,0;1,0;0,1;0,0;0,0];
B=[1,0,0;0,1,0;0,0,1;0,0,0;0,0,0];
H=eye(5,5);
C=[0,-g,0,0,0;g,0,0,0,0];
d21=[0,0,0,1,0;0,0,0,0,1];
d22=zeros(2,5);
lx=are(A,B*B'-(G*G')/(r*r),H'*H)
ly=are(A',C'*C-H'*H/(r*r),G*G')
K=-B'*lx;
Kd=(G'*lx)/r^2
L=inv(eye(5,5)-ly*lx/r^2)*ly*C'
p=eig(A+B*K+G*Kd)
p=norm(ly*lx)
p=det(ly)
TT=eye(3,3);
for i=1:8000;
   wt=TT*wb;
   at=TT*ab;
   for j=1:3;   
      wt(j,1)=wt(j,1);
      at(j,1)=at(j,1)+0.001*(rand(1)-0.5);
   end;  
   Y=[at(1,1),at(2,1)]';
   U=K*X;
   w=wt-wiet-U;
   WW=[0,-w(3),w(2);w(3),0,-w(1);-w(2),w(1),0];
   TT=WW*t+TT;
   T0=TT;
   for j=1:10;
     TT=TT-0.5*(TT*T0'*TT-T0);
   end;   
   error=Kd*X;
   for j=1:5;
   data(i,j)=X(j);
   end;
   X=(A*X+B*U+G*error+L*(Y-C*X))*t+X;   
end;
n=1:8000;
%plot(n*t,data(n,1),n*t,data(n,2),n*t,data(n,3),n*t,data(n,4),n*t,data(n,5));
plot(n*t,data(n,3),n*t,data(n,3),n*t,data(n,3));
zheyao=asin(TT(3,2))*180/pi
zhong=atan(-TT(3,1)/TT(3,3))*180/pi
souxiang=atan(-TT(1,2)/TT(2,2))*180/pi
error(1)*3600*180/pi


L
K
Kd