kalman2c.m

这些程序都是经过本人和许许多多工作过学习过的老师同学

%*********************************************求系统2c的秩**********************

clear all;

%重力加速度

g=9.80;    

%地速分量

Vx=8; 
Vy=8;
Vz=0; 

%地球半径

R=6371020;

%纬度角

angle=pi/4; 

%地球自转角速度

Wie=7.2921158e-5;

%加速度计的漂移误差

ax=10e-4*g;
ay=10e-4*g; 

%陀螺仪漂移误差

ex=0.01*pi/(180*3600); 
ey=0.01*pi/(180*3600); 
ez=0.01*pi/(180*3600);

%所测的比力信息

fx=(2*Wie*cos(angle)+Vx/R)*Vz-(2*Wie*sin(angle)+Vx*tan(angle)/R)*Vy;
fy=(2*Wie*sin(angle)+Vx*tan(angle)/R)*Vx+Vy*Vz/R;    
fz=-(2*Wie*cos(angle)+Vx/R)*Vx-Vy^2/R+g;

%求出A阵

A=zeros(12);

A(1,2)= Vx*tan(angle)/R+Wie*sin(angle);
A(1,3)=-Vx/R-Wie*cos(angle);
A(1,8)=-1/R;
A(2,1)=-Vx*tan(angle)/R-Wie*sin(angle);
A(2,3)=-Vy/R;
A(2,7)=1/R;
A(2,9)=-Wie*sin(angle);
A(3,1)=Vx/R+Wie*cos(angle);
A(3,2)=Vy/R;
A(3,7)=tan(angle)/R;
A(3,9)=Wie*cos(angle)+Vx*sec(angle)^2/R;
A(4,10)=1;
A(5,11)=1;
A(6,12)=1;
A(7,2)=-fz;
A(7,3)=fy; 
A(7,7)=(Vy*tan(angle)-Vz)/R;
A(7,8)=2*Wie*sin(angle)+Vx*tan(angle)/R; 
A(7,9)=2*Wie*sin(angle)*Vz+(2*Wie*cos(angle)+Vx*sec(angle)^2/R)*Vy;
A(8,1)=fz;
A(8,3)=-fx;
A(8,7)=-2*Wie*sin(angle)-2*Vx*tan(angle)/R;
A(8,8)=-Vz/R;
A(8,9)=-2*Wie*cos(angle)*Vx-Vx^2*sec(angle)^2/R;
A(9,8)=1/R;
%求出C阵

C=zeros(5,12);

C(1,7)=1;
C(2,8)=1;
C(3,1)=-1;
C(3,4)=1;
C(4,2)=-1;
C(4,5)=1;
C(5,3)=-1;
C(5,6)=1;

%求出秩

E=[C;C*A;C*A^2;C*A^3;C*A^4;C*A^5;C*A^6;C*A^7;C*A^8;C*A^9;C*A^10;C*A^11];
y=rank(E);
fprintf('%d',y);
[U,S,V]=svd(E);

%系统离散化

%系统方程离散化

B=zeros(12,1);
D=zeros(5,1);
[a,b,c,d]=c2dm(A,B,C,D,0.1,'foh');

%kalman滤波初始化

estX=zeros(12,301);
realX=zeros(12,301);
PP=zeros(12,12,300);
P=zeros(12,12,301);
K=zeros(12,5,301);
Z=zeros(5,301);
R=zeros(5);

R(1,1)=0.01;
R(2,2)=0.01;
R(3,3)=0.01;
R(4,4)=0.01;
R(5,5)=0.01;

%X的初始值

estX(:,1)=[0 0 0 0 0 0 0 0 0 0 0 0 ]';
realX(:,1)=[0 0 0 0 0 0 0 0 0 0 0 0 ]';

%协方差矩阵的初始值

P(1,1,1)=(pi/180)^2;
P(2,2,1)=(pi/180)^2;
P(3,3,1)=(pi/180)^2;
P(4,4,1)=(pi/180)^2;
P(5,5,1)=(pi/180)^2;
P(6,6,1)=(pi/180)^2;
P(7,7,1)=0.01;
P(8,8,1)=0.01;
P(9,9,1)=0;
P(10,10,1)=0.2;
P(11,11,1)=0.2;
P(12,12,1)=0.2;

for i=2:301
    PP(:,:,i)=a*P(:,:,i-1)*a';
    K(:,:,i)=PP(:,:,i)*c'*inv(c*PP(:,:,i)*c'+R);
    realX(:,i)=a*realX(:,i-1);
    Z(:,i)=c*realX(:,i)+wgn(5,1,0.1);
    estX(:,i)=a*estX(:,i-1)+K(:,:,i)*(Z(:,i)-c*a*estX(:,i-1));
    P(:,:,i)=PP(:,:,i)-K(:,:,i)*c*PP(:,:,i);
end

for i=1:301
    est1(i)=estX(1,i);
    est2(i)=estX(2,i);
    est3(i)=estX(3,i);
    est4(i)=estX(4,i);
    est5(i)=estX(5,i);
    est6(i)=estX(6,i);
    est7(i)=estX(7,i);
    est8(i)=estX(8,i);
    est9(i)=estX(9,i);
    est10(i)=estX(10,i);
    est11(i)=estX(11,i);
    est12(i)=estX(12,i);
end

%画出曲线图

t=0:10:3000;

figure(1)

subplot(3,1,1)
plot(t,est1,'b');
title('x1');
grid on;
subplot(3,1,2)
plot(t,est2,'b');
title('x2');
grid on;
subplot(3,1,3)
plot(t,est3,'b');
title('x3');
grid on;

figure(2)

subplot(3,1,1)
plot(t,est4,'b');
title('x4');
grid on;
subplot(3,1,2)
plot(t,est5,'b');
title('x5');
grid on;
subplot(3,1,3)
plot(t,est6,'b');
title('x6');
grid on;

figure(3)

subplot(3,1,1)
plot(t,est7,'b');
title('x7');
grid on;
subplot(3,1,2)
plot(t,est8,'b');
title('x8');
grid on;
subplot(3,1,3)
plot(t,est9,'b');
title('x9');
grid on;

figure(4)

subplot(3,1,1)
plot(t,est10,'b');
title('x13');
grid on;
subplot(3,1,2)
plot(t,est11,'b');
title('x14');
grid on;
subplot(3,1,3)
plot(t,est12,'b');
title('x15');
grid on;