卡尔曼滤波做物体轨迹估计【错误】求救

JUVE熊

小弟刚开始看卡尔曼滤波这一块。最近做一个物体轨迹的估计。看了[基于卡尔曼滤波预测的移动机器人矢量场矩形法避障研究]这片论文后仿照其中的卡尔曼滤波一章中的系统方程和观测方程，自己写了下面的程序。

clear
%假设物体从(0,10)位置开始X方向做速度为1的匀速运动，Y方向做初速为0加速度
%为0.16的加速度运动，T=0.2秒采样一次%
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');


上面的程序出来的结果根本不是抛物线而是比较直的线。。。。不知道为啥啊。。。。  求大神解救啊