lx3.m

组合导航的程序

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.000,0,0]';
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]
B=[1,0,0;0,1,0;0,0,1;0,0,0;0,0,0];
C=[0,-g,0,0,0;g,0,0,0,0];
m=[B,A*B,A^2*B,A^3*B];%可控
rank(m)
n=[C;C*A;C*A*A;C*A*A];%可观
rank(n)
%控制器的设计

A1=[zeros(2,2),C;zeros(5,2),A]
B1=[zeros(2,3);B]
point=-0.5+sqrt(-3)/2
point1=-0.5-sqrt(-3)/2
point2=-0.5
P=[point,point1,point,point1,point2];
L=place(A,B,P)
%观测器的设计
point=-0.2
point1=-0.2
point2=-0.001
point3=-0.0001
P1=[point,point1,point2,point2,point3];
K=place(A',C',P1)
P=eig(A-K'*C)
%仿真
sum=[0,0]';
TT=eye(3,3);
for i=1:3000
   wt=TT*wb;
   at=TT*ab;
   for j=1:3;   
      wt(j,1)=wt(j,1);
      at(j,1)=at(j,1)+0.0002*(rand(1)-0.5);
   end;  
   Y=[at(1,1),at(2,1)]';
   sum=sum+Y; 
   U=-L*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;
   X=(A*X+B*U+K'*(Y-C*X))*t+X;
   for j=1:3;
      data(i,j)=X(j);
      data1(i,j)=U(j);
   end;
end;
n=1:3000;
plot(n*t,data(n,3),n*t,data1(n,3));
0.01/g*180/pi
zheyao=asin(TT(3,2))*180/pi
zhong=atan(-TT(3,1)/TT(3,3))*180/pi
souxian=atan(-TT(1,2)/TT(2,2))*180/pi
A
B
C
-L
K'