kalman2.m

请不要用做非法用途

clear all;
h=50.0;
Re=6378393.0;
g0=9.78049;   g=g0*(1.0-2.0*h/Re);
pi=3.1415926; k=pi/180.0;
phi=45.7796;  phi=phi*k;
wie=7.2921185e-5;
w=[0;cos(phi);sin(phi)]*wie;
T=eye(3);
t=30;  step=0.8;  time=(1.0*t*60.0)/step;
dw=0.01*pi/180.0/3600.0;  df=1.0e-4*g;  dphi=1.0*pi/180.0;
F=[ 0          2.0*w(2)   0        -g      0       T(1,1)  T(1,2)   0         0        0;
    -2*w(2)    0          g        0       0       T(2,1)  T(2,2)   0         0        0;
    0          0          0        w(2)    -w(1)   0       0        T(1,1)    T(1,2)   T(1,3);
    0          0          -w(2)    0       0       0       0        T(2,1)    T(2,2)   T(2,3);
    0          0          w(1)     0       0       0       0        T(3,1)    T(3,2)   T(3,3);
    0          0          0        0       0       0		  0		  0			0         0;
    0          0          0        0       0       0		  0		  0			0         0;
    0          0          0        0       0       0		  0		  0			0         0;
    0          0          0        0       0       0		  0		  0			0         0;
    0          0          0        0       0       0		  0		  0			0         0 ];
 A=eye(10);   I=eye(10);
 A=I+step*F;
 H=[1.0     0.0     0.0     0.0    0.0     0.0     0.0   0.0     0.0    0.0;
    0.0     1.0     0.0     0.0    0.0     0.0     0.0   0.0     0.0    0.0];
 P=zeros(10);
 P(1,1)=power(0.1,2);        P(2,2)=power(0.1,2);  
 P(3,3)=power(0.6*dphi,2);   P(4,4)=power(0.6*dphi,2);    P(5,5)=power(1.0*dphi,2);
 P(6,6)=power(df,2);         P(7,7)=power(df,2);    
 P(8,8)=power(2.0*dw,2);     P(9,9)=power(2.0*dw,2);      P(10,10)=power(2.0*dw,2);
 Q=zeros(10);
 Q(1,1)=power(0.5*df,2);     Q(2,2)=power(0.5*df,2);
 Q(3,3)=power(dw,2);         Q(4,4)=power(dw,2);          Q(5,5)=power(dw,2);
 R=zeros(2);
 R(1,1)=power(0.1,2);        R(2,2)=power(0.1,2);
 X=zeros(10,1);
 AT=A';     HT=H';
 Q=(Q+A*Q*AT)*step/2.0;
 Z=zeros(2,1);
 K=zeros(10,2);
 Ttmp=zeros(3,3);
 C=zeros(3,3);
 data=zeros(2500,1);
 for l=1:1:(time-1)
    Z(1,1)=randn(1,1)/10.0;
    z(2,1)=randn(1,1)/10.0;
    X=A*X;
    P=A*P*AT+Q;
    K=P*HT*inv(H*P*HT+R);
    X=X+K*(Z-H*X);
    P=(I-K*H)*P;
    Ttmp=T;
    C(1,1)=1.0;      C(1,2)=-X(5,1);   C(1,3)=X(4,1);
    C(2,1)=X(5,1);   C(2,2)=1.0;       C(2,3)=-X(3,1);
    C(3,1)=-X(4,1);  C(3,2)=X(3,1);    C(3,3)=1.0;
    T=C*Ttmp;
    F(1,6)=T(1,1);   F(1,7)=T(1,2);
    F(2,6)=T(1,1);   F(2,7)=T(1,2);
    F(3,8)=T(1,1);   F(3,9)=T(1,2);   F(5,10)=T(1,3);
    F(4,8)=T(2,1);   F(4,9)=T(2,2);   F(4,10)=T(2,3);
    F(5,8)=T(3,1);   F(5,9)=T(3,2);   F(5,10)=T(3,3);
    A=I+step*F;
    AT=A';
    Q=(Q+A*Q*AT)*step/2.0;
    data(l,1)=X(5,1);
%    data(l,1)=0;
 end
 n=1:10:2500;
    plot(n,data(n,1));