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

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

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

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

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

%间隔时间步长.每秒30T = 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]);

最终结果:
在这里插入图片描述

在这里插入图片描述
在这里插入图片描述
参考资料:卡尔曼滤波(一)线性卡尔曼滤波算法及示例

  • 4
    点赞
  • 54
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值