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.1,0.1,g]';

X=[0.1/g,-0.1/g,0.00,0,0]';
temp=zeros(5);
r=11000;
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=zeros(3,5);
H(1,1)=0.1;
H(2,2)=0.1;
H(3,3)=0.1;
H(4,4)=0.001;
H(5,5)=0.001;

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.0001*(rand(1)-0.5);
   end;  
   at(1,1)=at(1,1)+0.001*(rand(1)-0.5);
   at(2,1)=at(2,1)+0.001*(rand(1)-0.5);
   at(3,1)=at(3,1)+0.001*(rand(1)-0.5);
   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; 
   attitude(1)=asin(TT(3,2))*180/pi;
   attitude(2)=atan(-TT(3,1)/TT(3,3))*180/pi;
   attitude(3)=atan(-TT(1,2)/TT(2,2))*180/pi;
   for j=1:3;
      data1(i,j)=attitude(j);
   end;

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));
subplot(2,2,1)
plot(n*t,data1(n,1));
subplot(2,2,2)
plot(n*t,data1(n,2));
subplot(2,2,3)
plot(n*t,data1(n,3));


zheyao=asin(TT(3,2))*180/pi
zhong=atan(-TT(3,1)/TT(3,3))*180/pi*tan(wd*pi/180)
souxiang=atan(-TT(1,2)/TT(2,2))*180/pi
error(1)*3600*180/pi
L
K
Kd