使用matlab对输入数据进行卡尔曼滤波

使用matlab对输入数据进行卡尔曼滤波。

简单说明一下程序中的数据。假设一辆汽车从初始点（0，10）开始行驶，初始速度沿y轴正方向10m/s。然后在观测途中向右先加速再减速变换车道。

整个过程其实有x轴坐标，y轴坐标，x轴速度，y轴速度以及x轴的加速度这5个物理量。但在卡尔曼滤波时我只用了x轴坐标，y轴坐标，x轴速度，y轴速度这4个量，相当于将x轴的人为施加的加速度当作了噪声处理。人为施加的加速度当然不符合高斯分布，有违卡尔曼滤波的假设，这里只是为现实应用直接处理的。有兴趣的可以把x轴的加速度这个量也纳入到卡尔曼滤波中。

clc
clear
close all
%% 汽车变道数据生成
% 汽车初始位置为（0，10）,初始x方向速度为0，y方向速度为5m/s，向前匀速运动
% 在第400-600帧时，向右加速，加速度为0.05m/s^2
% 在第601-610帧时，停止加速
% 在第611-810帧时，向左加速，加速度为0.05m/s^2，正好使得x轴方向速度为0，完成变道

%间隔时间步长.每秒30帧
T = 1/30;
%总的观测帧数目
totaltime = 1600;                             
% 状态转移矩阵F
f_1 = [1 T T^2/2;0 1 T;0 0 1];
f_2 = [1 T;0 1];
F = blkdiag(f_1,f_2);
% [初始x坐标， 初始x速度， 初始x加速度， 初始y坐标， 初始y速度]
X(:,1) = [0 0 0 10 5]';
% 观测矩阵H，观测到x坐标，x速度， y坐标， y速度四个量
H = [1 0 0 0 0;0 1 0 0 0;0 0 0 1 0; 0 0 0 0 1];
% 过程噪声，可能路面比较滑，风力等影响，对下个时刻的真实状态造成影响
sigma = 0.00005;
Q = diag([sigma sigma/T 0 sigma sigma/T]);
% 传感器的观测噪声矩阵R
R = diag([0.25, 0.25, 0.25, 0.25]);
% 观测到的数据
Z(:,1) = H * X(:,1) + sqrtm(R) * randn(4,1);  

for i = 2 : totaltime
    % 当前时刻的状态由上一时刻的状态变换过来，加上过程噪声Q
    X(:,i) = F * X(:, i-1) + Q * randn(5,1);
    % 司机手动输入导致加速度改变
    % 400-600 车辆向右加速变道
    if i>400 && i<=600
        X(3,i) = 0.05;
    end
    % 600-610 车辆匀速变道
    if i>600 && i<=610
        X(3,i) = 0;
    end
    % 610-810 车辆减速变道
    if i>610 && i<=810
        X(3,i) = -0.05;
    end
    % 810以后 变道完成
    if i>810
        X(3,i) = 0;
    end
    % 测量结果由当前时刻的真实数据经过测量矩阵H，然后加上观测噪声矩阵R得到
    Z(:,i) = H * X(:, i) + R * randn(4,1); 
end

%% 卡尔曼滤波
% 卡尔曼滤波后的目标结果[x坐标， x速度， y坐标， y速度]
Xkf(:,1) = [X(1:2,1); X(4:5,1)];
Xkf(:,1) = [10; 2; 15; 8];
f_1 = [1 T;0 1];
f_2 = [1 T;0 1];
F = blkdiag(f_1,f_2);
sigma = 0.0005;
% Q表示过程噪声。由于Q对整个系统存在影响，但又不能太确定对系统的影响有多大。
% 工程上，我们一般将Q设置为单位矩阵参与运算
Q = diag([sigma sigma sigma sigma]);
H = eye(4);
% Xkf(:,1) = [3; 0; 0; 20; 5];
P0 = 100*eye(4);
for i = 2: totaltime
    % 卡尔曼滤波7公式（y和S是中间变量，简化后是卡尔曼滤波5公式）
    % 根据上一时刻的状态预测这一时刻的状态
    Xn = F * Xkf(:,i-1);
    % P矩阵表示系统的不确定程度，这个不确定程度，在卡尔曼滤波器初始化时会很大，
    % 随着越来越多的数据注入滤波器中，不确定程度会变小，P的专业术语叫状态协方差矩阵
    P1 = F * P0 * F' + Q;
    % y代表实际观测到的测量值Z(:,i)与预测值H * Xn之间差值
    y = Z(:,i) - H * Xn;
    % 卡尔曼增益K,预测的状态与当前时刻的测量值进行加权,就是y的权值(S是临时变量)
    S = H * P1 * H' + R;
    K = P1 * H' * inv(S);
    % 完成对当前状态向量x的更新,不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声
    Xkf(:,i) = Xn + K * y;
    % 根据卡尔曼增益，更新系统的不确定度P，用于下一个周期的运算
    P0 = (eye(4) - K * H) * P1;
end

%% 最终结果
figure
hold on;
box on;
grid on
plot(Z(1,:),Z(3,:),'b.');
plot(X(1,:),X(4,:),'-r');
plot(Xkf(1,:),Xkf(3,:),'-k');
legend('观测点','真实轨迹','滤波轨迹')
xlabel('X');
ylabel('Y');
daspect([0.03 1 1]);

figure
hold on;
box on;
grid on
plot(Z(2,:),Z(4,:),'-b');
plot(Xkf(2,:),Xkf(4,:),'-k');
plot(X(2,:),X(5,:),'-r');
legend('观测速度','滤波速度','真实速度')
xlabel('X方向速度');
ylabel('Y方向速度');
daspect([1 1 1]);

最终结果：

参考资料：卡尔曼滤波（一）和线性卡尔曼滤波算法及示例