inva.m

组合导航的程序

wie=7.29e-5;
n=wie*cos(3.1415926/4);
d=-wie*sin(3.1415926/4);
c=[1 0 0
   0 1 0
   0 0 1]
a=[0 2*d 0 9.8 0 
   -2*d 0 -9.8 0 0
   0 0 0 d 0  
   0 0 -d 0 n 
   0 0 0 -n 0];
b1=[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]
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]
c1=[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
   1
   2
   5];
m=[c1
   c1*a
   c1*a*a
   c1*a*a*a
   c1*a*a*a*a
   c1*a*a*a*a*a
   c1*a*a*a*a*a*a
];
rank(m);
det(a'+a-0.05*c1);
e=1/(1/1+2.05);
f=-1/(2*e)*c1;
aa=a+f;
for n=1:5000 
   w=0.01+0.1*randn(5,1);
   x=(aa*x+w)*0.01+x;
   y1(n)=x(1); 
   y2(n)=x(2);   
   y3(n)=x(3);   
   y4(n)=x(4);   
   y5(n)=x(5);   
end
n=1:1:5000;  
   plot(n,y1(n),'r',n,y2(n),'r',n,y3(n),'r',n,y4(n),'r',n,y5(n));