Kalman 滤波的问题

针对x，y两个方向上的运动，利用卡尔曼滤波器分别估计x、vx、y、vy。观测值是x，y向独立观测距离，我写了下面的代码，建立了一个4状态的卡尔曼滤波器。状态向量是[x xDot y yDot]'，利用x，y向的测量值进行估计，代码如下。但是为何估计结果中y向估计总是不对呢，有哪位大神可以给指导一下。小弟感激不尽！

已验证的结论如下：1、估计结果错误与状态的初始化值、协方差矩阵无关；2、添加过程噪声也无法解决问题；3，对速度的估计不知为啥估计出来的y向速度总是保持与x向的速度一样。

 

clear;
Ts=0.1;
t=0:Ts:25;
x=6*t+1;
y=4*t+5;
num  =  size(x,2);
xS=x+normrnd(0,0.1,1,num);
yS=x+normrnd(0,0.1,1,num);
ux  =  0;
uy  =  0 ;
PHI  = [ 1 Ts 0 0;0 1 0 0;0 0 1 Ts;0 0  0 1] ;    %传递矩阵
Qk=[Ts^3/3*ux  Ts^2/2*ux  0 0;Ts^2/2*ux  Ts*ux 0 0;0 0 Ts^3/3*uy Ts^2/2*uy;0 0 Ts^2/2*uy Ts*uy];

H  =  [1 0 0 0 ;0 0 1 0];
I=eye(4,4);

%滤波器初始化
stateH  =  zeros(4,1);
stateH(1,1)  =  1 + 0 ;
stateH(2,1)  =  0 + 0 ;
stateH(3,1)  =  0 + 0;
stateH(4,1)  =  2 + 0;     %状态初始化

Rk=[0.1^2  0; 0  0.1^2];
Pk=zeros(4,4);
Pk(1,1)  =  1 ;
Pk(2,2)  =  1;
Pk(3,3)  =  1;
Pk(4,4)  =  1 ;    %协方差初始化。
xH  =  zeros(1,num);
xDotH  =  zeros(1,num);
yH  =  zeros(1,num);
yDotH  =  zeros(1,num);

%状态测量实际残差
xErr  =  zeros(1,num);
xDotErr  =  zeros(1,num);
yErr  =  zeros(1,num);
yDotErr  =  zeros(1,num);

for k  =  1:num
    Mk  =  PHI * Pk * PHI' + Qk;
    Res_Theory  =  H * Mk * H' + Rk;
    Kk  =  Mk * H' * inv(Res_Theory);
    stateM(1,1)=xS(k);
    stateM(2,1)=yS(k);
    Resk  =  stateM - H * PHI * stateH;
    stateH  =  PHI * stateH + Kk * Resk;
    Pk  =  (I - Kk * H) * Mk;  
    xH(k)  =  stateH(1,1);

    xDotH(k)  =  stateH(2,1);
    yH(k)  =  stateH(3,1);
    yDotH(k)  =  stateH(4,1);     %获取状态估计值
    
    xErr(k)  =  sqrt(Pk(1,1));
    yErr(k)  =  sqrt(Pk(3,3));  %获取状态测量值理论残差
     xS(k)  =  stateM(1,1);
     yS(k)  =  stateM(2,1);
end
%绘图分析
clf;
close all;
figure;
plot((1:k)*0.1,x,(1:k)*0.1,xH);
legend('x距离真实值','x距离估计值');
figure;
plot((1:k)*0.1,y,(1:k)*0.1,yH);
legend('y距离真实值','y距离估计值');
figure;
plot((1:k)*0.1,xH-x,(1:k)*0.1,xErr,(1:k)*0.1,-xErr);
legend('x距离实际残差','x距离理论残差');
figure;
plot((1:k)*0.1,yH-y,(1:k)*0.1,yErr,(1:k)*0.1,-yErr);
legend('y距离实际残差','y距离理论残差');