hfine.m

来自「组合导航的程序」· M 代码 · 共 64 行

wd=45.6679;
PI=3.1415926;
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.5*PI/180.0
   0.2*PI/180.0
   1*PI/180.0];
f=-1/(2*e)*p;
a=a+f;
for i=1:1000
   v=[5*0.0001*g+5*0.001*g*(rand(1)-0.5)
      5*0.0001*g+5*0.001*g*(rand(1)-0.5)
      0.01*PI/(180*3600.0)+1*PI/(180*3600.0)*(rand(1)-0.5)
      0.01*PI/(180*3600.0)+1*PI/(180*3600.0)*(rand(1)-0.5)
      0.01*PI/(180*3600.0)+1*PI/(180*3600.0)*(rand(1)-0.5)];
   dx=a*x+v*10000;
   x=dx*t+x;
   data(i)=x(3);
   data1(i)=x(4);   
   data2(i)=x(5);
end;
n=1:1000;
plot(n*0.01,data(n),n*0.01,data1(n),n*0.01,data2(n));