直角坐标系卡尔曼滤波目标跟踪

%kalman滤波模拟
%x，y    0~10000
%速度  100m/s
F=[1 1 0 0;
    0 1 0 0;
    0 0 1 1;
    0 0 0 1];
%过程噪声
V=[];
H=[1 0 0 0;0 0 1 0];
sigmaQ=0.1;
sigmaR=10;
Q=[sigmaQ 0 0 0;
    0 sigmaQ 0 0;
    0 0 sigmaQ 0;
    0 0 0 sigmaQ];
sigmaQq=0.1;
Q_=[sigmaQq 0 0 0;
    0 sigmaQq 0 0;
    0 0 sigmaQq 0;
    0 0 0 sigmaQq];
sigmaRr=0;
R_=[sigmaRr 0;0 sigmaRr];
R=[sigmaR 0;0 sigmaR];
x=[];
z=[];
%生成轨迹
err（1:611:4）=0;
p_err（1:611:4）=0;
k（1:602）=0;
for j=1:1:150
    X=[0203]‘;
    for i=1:1:61
        x（i:）=X;
        z（i:）=H*X+normrnd（0sqrt（sigmaR）21）;
        X=F*X+normrnd（0sqrt（sigmaQ）41）;
    end
    %卡尔曼滤波
    k_x=[z（21） z（21）-z（11） z（22） z（22