ndslndsl
2009-05-14, 15:23
:( 我编好了一个仿真程序，能运行，可是结果不对。因为我仿真的是一个匀速运动的卡尔曼滤波。但是仿真出来后速度确实斜线上升的。不知道是什么原因，而且协方差pk不是收敛的，求高手解答！
%卡尔曼滤波仿真
clear;
clc;
A=eye(4);
T=0.5;
A(5)=T;
A(15)=T; %转移矩阵
xe=0,ve=5,xn=0,vn=5;
X0=[xe;ve;xn;vn];%状态变量
X=[X0 zeros(4,99)];
H=[0 T*ve/sqrt(ve^2+vn^2) 0 T*vn/sqrt(ve^2+vn^2);0 vn/(vn^2+ve^2) 0 -ve/(vn^2+ve^2)];
P=[10 0 0 0;0 1 0 0;0 0 10 0;0 0 0 1];%初始的协方差
Q=[0.5 0 0 0;0 0.5 0 0;0 0 0.5 0;0 0 0 0.5];
R=[0.5 0;0 0.5];
Z=zeros(2,99);
for n=1:99
if n==1
Z(:,1)=[5*sqrt(2)*0.5+wgn(1,1,1);pi/4+0.01*wgn(1,1,1)];
X(:,1)=[Z(1,1)*sin(Z(2,1));5;Z(1,1)*cos(Z(2,1));5]
else n>1
t=n*T;
Z(:,n)=[5*(2^0.5)*t+wgn(1,1,1);pi/4+0.01*wgn(1,1,1)]%观测信息
X2(:,n)=A*X(:,n-1);%求先验估计
P2=A*P*A'+Q;%求先验估计协方差
K=P2*H'*inv(H*P2*H'+R);%求最佳增益
X(:,n)=X2(:,n)+K*(Z(:,n)-H*X2(:,n));%求估计值
P=(eye(4)-K*H)*P2%求协方差
end
end
figure(1);
t=0.5:0.5:50;
plot(t,X(1,:),'r+');xlabel('时间:s');ylabel('xe:m');grid on;title('xe的滤波后轨迹');
figure(2);
plot(t,X(3,:),'r*');xlabel('时间:s');ylabel('ye:m');grid on ;title('ye的滤波后轨迹');
figure(3);
n=1:99;
plot(n,Z(1,n).*cos(Z(2,n)),'b*');title('ye的滤波前轨迹');
%卡尔曼滤波仿真
clear;
clc;
A=eye(4);
T=0.5;
A(5)=T;
A(15)=T; %转移矩阵
xe=0,ve=5,xn=0,vn=5;
X0=[xe;ve;xn;vn];%状态变量
X=[X0 zeros(4,99)];
H=[0 T*ve/sqrt(ve^2+vn^2) 0 T*vn/sqrt(ve^2+vn^2);0 vn/(vn^2+ve^2) 0 -ve/(vn^2+ve^2)];
P=[10 0 0 0;0 1 0 0;0 0 10 0;0 0 0 1];%初始的协方差
Q=[0.5 0 0 0;0 0.5 0 0;0 0 0.5 0;0 0 0 0.5];
R=[0.5 0;0 0.5];
Z=zeros(2,99);
for n=1:99
if n==1
Z(:,1)=[5*sqrt(2)*0.5+wgn(1,1,1);pi/4+0.01*wgn(1,1,1)];
X(:,1)=[Z(1,1)*sin(Z(2,1));5;Z(1,1)*cos(Z(2,1));5]
else n>1
t=n*T;
Z(:,n)=[5*(2^0.5)*t+wgn(1,1,1);pi/4+0.01*wgn(1,1,1)]%观测信息
X2(:,n)=A*X(:,n-1);%求先验估计
P2=A*P*A'+Q;%求先验估计协方差
K=P2*H'*inv(H*P2*H'+R);%求最佳增益
X(:,n)=X2(:,n)+K*(Z(:,n)-H*X2(:,n));%求估计值
P=(eye(4)-K*H)*P2%求协方差
end
end
figure(1);
t=0.5:0.5:50;
plot(t,X(1,:),'r+');xlabel('时间:s');ylabel('xe:m');grid on;title('xe的滤波后轨迹');
figure(2);
plot(t,X(3,:),'r*');xlabel('时间:s');ylabel('ye:m');grid on ;title('ye的滤波后轨迹');
figure(3);
n=1:99;
plot(n,Z(1,n).*cos(Z(2,n)),'b*');title('ye的滤波前轨迹');