在matlab中实现二维卡尔曼滤波

在matlab中实现二维卡尔曼滤波，模型采用简单的恒速模型。

N = 20;
x = zeros（1N）;
y = zeros（1N）;
xv = zeros（1N）;
yv = zeros（1N）;
xa = zeros（1N）;
ya = zeros（1N）;
x（1） = 15; y（1） = 15; xv（1） = 5; yv（1） = 0; xa（1） = 1; ya（1） = 0.5;
X = [x; y; xv; yv; xa; ya];
T = 1;             %测量周期为1
A = [1 0 T 0 T^2/2 0;0 1 0 T 0 T^2/2;0 0 1 0 T 0;0 0 0 1 0 T;0 0 0 0 1 0;0 0 0 0 0 1];
Z = [x; y];
H = [1 0 0 0 0 0;0 1 0 0 0 0];
w_sigma = 10.0;       %w为过程激励噪声，是正态分布的白噪声，期望为0，标准偏移为w_sigma;
v_sigma = 10.0;       %v为观测噪声，也是正态分布的白噪声，期望为0，标准偏移为v_sigma;

for k = 2:N;
    W = normrnd（0w_sigma6N）;
    V = normrnd（0v_sigma2N）;
    X（:k） = A*X（:k-1）+W（:k-1）;      %真实值   
    Z（:k） = H*X（: k）+

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1415  2012-04-23 10:00  kalman.m

----------- ---------  ---------- -----  ----

                 1415                    1