question1_ab.m

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

clc;
clear,T = 1;
x0 = [0; 1; 0; 1];
w=[0.01; 0.01];
v=[100; 100];
n=1000;
[x, y] = cv(x0,w,v,T,n);
N = 1 : 1001;
A = [1, T, 0, 0; 0, 1, 0, 0; 0, 0, 1, T; 0, 0, 0, 1];
a = 0.05;
b = 0.01;
re = ab(y, A, a, b, x0, T);
figure(1);
plot(x(1,:),x(3,:),'r');
hold on;
plot(re(1,:),re(3,:),'b');
xlabel('x1'); ylabel('x2'); title('ab滤波器滤波后的位移'); 
grid on;
figure(2);
subplot(2,1,1); plot(N, x(2,:), 'r', N, re(2,:), 'b');
title('ab滤波器滤波后的速度'); 
ylabel('v1'); 
grid on; 
subplot(2,1,2); plot(N, x(4,:), 'r', N, re(4,:), 'b');
ylabel('v2'); 
grid on;