卡尔曼滤波轨迹估计

最近开始学习用卡尔曼滤波KF进行目标轨迹估计预测。我的模型是仿照附件里论文的卡尔曼滤波那一章的模型设置的。就是一个物体按一抛物线运动。

clear
%假设物体从(0,10)位置开始X方向做速度为1的匀速运动，Y方向做初速为0加速度
%为0.16的加速度运动，T=0.2秒采样一次%
%状态矩阵X=[X坐标 X速度 Y坐标 Y速度]T，观测矩阵Z与X相同，将加速度作为噪声。
a=[1,0.2,0,0;0,1,0,0;0,0,1,0.2;0,0,0,1];
b=[0.02,0,0,0;0,0.2,0,0;0,0,0.02,0;0,0,0,0.2];
Ax=0;
Ay=-0.16;
A=[Ax;Ax;Ay;Ay];
w1=randn(1,50)*sqrt(0.05);
w2=randn(1,50)*sqrt(0.05);
w=[w1;w1;w2;w2];
x(:,1)=[0,1,10,0];
z(:,1)=[0,1,10,0];
for k=2:50;
x(:,k)=a*x(:,k-1)+A;   % 真值
z(:,k)=x(:,k)+w(:,k-1);   %测量值
end
%plot(x(1,,x(3,,z(1,,z(3,,'r')%
Q=0.05*eye(4);
H=eye(4);          %观测方程Z=HX+R
R=0.05*eye(4);
I=eye(4);
Xt_1(:,1)=[0,1,10,0];   % X(0)
Pt_1=0.05*eye(4);    %P(0)
for N=1:50;
Pt_=a*Pt_1*a'+b*Q*b';   %Pt_为P(n|n-1)   Pt_1为P(n-1|n-1)
Kg=Pt_*H'*(H*Pt_*H'+R)^(-1);
Pt=(I-Kg*H)*Pt_;   %Pt为P(n|n)
Xt_=a*Xt_1;       %预测
Xt=Xt_+Kg*(z(:,k)-H*Xt_); %最优估计
Xyc(:,N)=Xt_; %输出预测
Xxz(:,N)=Xt;   %输出最优估计
Pt_1=Pt;   %更新P(n|n)
Xt_1=Xt;   %更新X(n|n)
end
plot(Xxz(1,,Xxz(3,,'r',x(1,,x(3,,Xyc(1,,Xyc(3,,'g');

出来的结果和真值、测量值都相差甚远几乎为直线。。。。。。不知为何。。如果方便的话，十分希望得到各位的回答，谢谢！







有错误：
Xt=Xt_+Kg*(z(:,k)-H*Xt_); %最优估计
这一行的z(:,k) 应为 z(:,N)