ab.m

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

function re = ab(y, A, a, b, x0, T);
[M, N] = size(y);
re(:, 1) = x0;
K = [a, 0; b/T, 0; 0, a; 0, b/T];  %ab滤波器参数
for i = 1 : 1 : N-1
    x_K = A * re(:, i);  %由状态方程得到观测值x(k+1|k)
    f = y(:, i+1) - [x_K(1, 1); x_K(3, 1)]; %新息(二维)
    re(:, i+1) = x_K + K * f;
end