kalman.m

本程序给出了ab滤波算法和Kalman滤波算法。 程序以叠加噪声的二维CV运动为例(包含轨迹突变情况)

function x = kalman(y, A, C, Q, R, p0, x0);
[M, N] = size(y);
x(:,1) = x0;
I = [1, 0, 0, 0; 0, 1, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1];
xk = x0;
pk = p0;
for i = 1 : 1 : N-1;
   xk1_k = A * xk;
   pk1_k = A * pk * A' + Q;
   kk1 = pk1_k * C' * inv(C * pk1_k * C' + R);
   xk1 = xk1_k + kk1 * (y(:, i + 1) - C * xk1_k);
   pk1 = (I - kk1 * C) * pk1_k;
   x(:, i+1) = xk1;
   pk = pk1;
   xk = xk1;
end;