hfineold.m

请不要用做非法用途

wd=45.6679;
PI=3.1415926;
dphi=PI/180;
wie=7.29e-5;
R=6378.393*1000;
g=9.8;
t=0.001;
wiev=wie*sin(wd*PI/180.0);
wien=wie*cos(wd*PI/180.0);
cc=[1 0 0 0 0
   0 1 0 0 0
   0 0 1 0 0
   0 0 0 1 0
   0 0 0 0 1];
wz=[0.1,0.1,0.1,0.1,0.1];
r=0.1;
e=1/(2.05+1/(r*r));
a=[0 2*wiev 0 -g 0
   -2*wiev 0 g 0 0 
   0 -1/R 0 wiev -wien 
   1/R 0 -wiev 0 0 
   (wiev/wien)/R 0 wien 0 0];
b1=[cc(1) cc(2) 0 0 0 
   cc(4) cc(5) 0 0 0 
   0 0 cc(1) cc(2) cc(3) 
   0 0 cc(4) cc(5) cc(6)
   0 0 cc(7) cc(8) cc(9)];
b2=[1 0 0 0 0
   0 1 0 0 0
   0 0 1 0 0
   0 0 0 1 0
   0 0 0 0 1];
h=[1 0 0 0 0
   0 1 0 0 0
   0 0 1 0 0
   0 0 0 1 0
   0 0 0 0 1];
p=[1 0 0 0 0
   0 1 0 0 0
   0 0 1 0 0
   0 0 0 1 0
   0 0 0 0 1];
x=[0.0
   0.0
   0.1*dphi
   0.2*dphi
   0.3*dphi];
f=-1/(2*e)*p;
a=a+f;
v=zeros(5,1);
for i=1:1000
   v(3,1)=0.001+0.01*randn(1);
   v(4,1)=0.001+0.01*randn(1);
   v(5,1)=0.001+0.01*randn(1);
   dx=a*x+v;
   x=dx*t+x;
   data(i)=x(3);
   data1(i)=x(4);   
   data2(i)=x(5);
end;
n=1:1000;
plot(n,data(n),n,data1(n),n,data2(n));