kalman.m

请不要用做非法用途

clear all
wd=45.6679;
PI=3.1415926;
wie=7.29e-5;
R=6378.393*1000;
g=9.8;
step=2;
t=30;
time=t*60/step;
dw=0.01*PI/180/3600;
df=1e-4*g;
dphi=PI/180;
w=[wie*cos(wd*dphi);wie*sin(wd*dphi)];
A=eye(10);
T=eye(3);
Temp=eye(3);
C=zeros(3);
 F=[0       2*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       -w(2)  0   w(1)    0       0     T(2,1) T(2,2) T(2,3);
     0       0        0    w(2)  0      0       0     T(1,1) T(1,2) T(1,3);
     0       0        0   -w(1)  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;];


Z=zeros(2,1);
HX=zeros(2,1);
HPHT=zeros(2);
Rinv=zeros(2);
KZ=zeros(10,1);
AX=zeros(10,1);
HP=zeros(2,10);
PHT=zeros(10,2);
AP=zeros(10);
APAT=zeros(10);
KH=zeros(10);
QT=zeros(10);
AQ=zeros(10);
AQAT=zeros(10);
X=zeros(10,1);
XK=zeros(10,1);
W=zeros(10,1);
H=zeros(2,10);
R=zeros(2);
P=zeros(10);
PK=zeros(10);
Q=zeros(10);
K=zeros(10,2);
data=zeros(1000,1);
data1=zeros(1000,1);
data2=zeros(1000,1);
H(1,1)=1;
H(2,2)=1;
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);

P=P*0.1;

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(1,1)=power(0.1,2);
R(2,2)=power(0.1,2);

G=eye(10);

HT=H';
AT=A';
Z(1)=100*randn(1)/1000;
Z(2)=100*randn(1)/1000;
for i=1:1000;
   XK=A*X;
   PK=A*P*AT+Q;
   K=PK*HT*inv(H*PK*HT+R);
   X=XK+K*(Z-H*XK);
   P=(eye(10)-K*H)*PK;
   Temp=T;
   C(1,1)=1.0;
   C(1,2)=-X(5);
   C(1,3)=X(4);
   C(2,1)=X(5);
   C(2,2)=1.0;
   C(2,3)=-X(3);
   C(3,1)=-X(4);
   C(3,2)=X(3);
   C(3,3)=1.0;
   T=C*Temp;
   F=[0       2*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       -w(2)  0   w(1)    0       0     T(2,1) T(2,2) T(2,3);
     0       0        0    w(2)  0      0       0     T(1,1) T(1,2) T(1,3);
     0       0        0   -w(1)  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)+step*F;
%  A1=A;
 [A,B]=c2d(F,G,step);
  AT=A';

  %Q=(Q+A*Q*AT)*step/2.0;
  data(i)=X(3);
  data1(i)=X(4);
  data2(i)=X(5);
  Z(1)=100*randn(1)/1000;
  Z(2)=100*randn(1)/1000;

end
figure(1);
%subplot(3,1,1);
n=1:1000;
plot(n,data(n)*180/pi*60);
ylabel('pitch/`');
xlabel('t/s');
figure(2);
%subplot(3,1,2);
plot(n,data1(n)*180/pi*60);
ylabel('roll/`');
xlabel('t/s');
figure(3);
%subplot(3,1,3);
plot(n,data2(n)*180/pi*60);
ylabel('yaw/`');
xlabel('t/s');