hfine1.m

请不要用做非法用途

wd=45.6679;
PI=3.1415926;
wie=7.29e-5;
R=6378.393*1000;
g=9.8;
t=0.001;
dw=0.01*PI/180/3600;
dphi=PI/180;
df=1e-4*g;
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=[df;df;dw;dw;dw];
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.3*dphi
   0.5*dphi
   1.0*dphi];
f=-1/(2*e)*p;
a=a+f;
 
 F1=zeros(2); 
 G1=zeros(2);  
 [F1,G1]=c2d(a,b1,t);
 v=zeros(5,1);

for i=1:1000
  % v(5,1)=0.0001*rand(1);
  %v(4,1)=0.0001*rand(1);
  %v(3,1)=0.0001*rand(1);
  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;
  x=F1*x+G1*wz+v;
   data(i)=x(3)/dphi;
   data1(i)=x(4)/dphi;   
   data2(i)=x(5)/dphi;
end;
figure(1);
n=1:1000;
plot(n/10,data(n));
figure(2);
n=1:1000;
plot(n/10,data1(n));
figure(3);
n=1:1000;
plot(n/10,data2(n));