one.m

请不要用做非法用途

clear all;
wd=45.6679;
PI=3.1415926;
wie=7.29e-5;
R=6378.393*1000;
dphi=PI/180;
g=9.8;
t=0.1;
dw=0.01*PI/180/3600;
df=1e-4*g;
kx=0.102;  ky=0.085;  kz=10;%347.7703;
kdx=0.4;   kdy=0.6;   kdz=0.6;
x=[0.6*dphi
   0];
y=[0.5*dphi
   1*dphi
   0
   0];
ux=[-dw
   kdx*df];
uy=[-dw
    -dw
    kdy*df
     0  ];
A1=[  0       kx
    -g*kdx   -kdx ];
A2=[0    -wie*cos(wd*PI/180.0)   -ky      0
     0             0               0      kz
    g*kdy          0              -kdy     0
    0             0               kdz   -kdz];
 B1=eye(2);    B2=eye(4);
 F1=zeros(2);  F2=zeros(4);
 G1=zeros(2);  G2=zeros(4);
 [F1,G1]=c2d(A1,B1,t);
 [F2,G2]=c2d(A2,B2,t);
for i=1:10000;
   x=F1*x+G1*ux;
   data1(i)=x(1)/dphi;
end; 
for i=1:10000;
   y=F2*y+G2*uy;
   data(i)=y(1)/dphi;
   data2(i)=y(2)/(dphi*10);
end;
figure(1);
n=1:10000;
plot(n/10,data(n));
figure(2);
n=1:10000;
plot(n/10,data1(n));
figure(3);
n=1:10000;
plot(n/10,data2(n));
figure(4);
n=1:10000;
plot(n/10,-1*data2(n));