关于对于车辆轨迹的卡尔曼滤波仿真以及自己对于卡尔曼滤波的理解

最新推荐文章于 2024-07-22 15:29:01 发布

写代码的小姜

最新推荐文章于 2024-07-22 15:29:01 发布

阅读量2.9k

点赞数 1

分类专栏：卡尔曼滤波文章标签：卡尔曼滤波算法车辆轨迹仿真

本文链接：https://blog.csdn.net/qq_42955211/article/details/116031568

版权

卡尔曼滤波专栏收录该内容

1 篇文章 0 订阅

订阅专栏

1. 理论

最近学习卡尔曼滤波，由于卡尔曼滤波的公式都很简单，这里仅仅是将最终公式贴在这里，他是由贝叶斯滤波在假设各变量为高斯分布且相互独立的情况下推出来的，公式如下：
预测公式
更新公式
以上公式是卡尔曼滤波（KF）的公式，对于扩展卡尔曼滤波无非就是状态转移矩阵的方程是非线性，我们仅仅针对非线性局部线性化，对其求雅可比矩阵，之后按照卡尔曼滤波公式进行计算即可。

2. 无人机轨迹预测与仿真（卡尔曼滤波）

2.1 卡尔曼滤波理解

总体来说仿真卡尔曼滤波就是在已知状态方程的情况下得到各矩阵的值，先算出加上高斯噪声的测量值，得到测量值以后，就可以根据卡尔曼公式，首先初始化，之后进行预测，得到这一时刻的预测值。预测之后得到这一时刻的更新值，也就是后验估计，之后依次迭代，最终画出来波形。
实际上，真正场景下的卡尔曼滤波，观测值可以直接通过传感器直接获取。预测值根据传感器与状态方程间接得到。
在PX4源码中就是根据IMU的数据间接计算去预测我们想要的姿态位置等状态，同时根据其他的一些传感器比如磁罗盘，GPS，外置视觉等或者理论推导得到我们的观测值，之后通过扩展卡尔曼滤波得到我们想要的状态。具体的解释请参照官方文档PX4中扩展卡尔曼滤波EKF的应用

2.2 轨迹仿真

（使用matlab2019进行仿真）

%%%扩展卡尔曼滤波仿真
close all;
clear;
clc;

rng(1000)%产生随机种子，以后rand会用到


%%  参数
Delta_t = 0.1; %采样时间，多传感器的时候使用短板采样时间
t_max = 100; %总的采样时间
k_max = t_max / Delta_t;%卡尔曼滤波一共递归多少次

V_c = 20; %车的初始速度
omega_c = 0; %车的初始角速度
H = [1 0 0; 0 1 0; 0 0 1];%观测与输入的矩阵，这里看成线性矩阵

Q = [Delta_t 0 0; 0 Delta_t 0; 0 0 Delta_t/180];%预测噪声的协方差矩阵
R = [100 0 0; 0 100 0; 0 0 0.01];%观测噪声的协方差矩阵


%%  初始化
m_0 = zeros(3, 1);%x_0的期望
P_0 = [1e6 0 0; 0 1e6 0; 0 0 pi^2];%x_0的协方差

x_0 = mvnrnd(m_0, P_0)'; %产生多元正态随机数 x_0是预测的三个变量的初始值随机生成的


%%  仿真数据初始化
xSequence = zeros(3, k_max+1);%预测的初始序列
ySequence = zeros(3, k_max+1);%观测的初始序列
x_hatSequence = zeros(3, k_max+1);%后验的初始序列
k_indexC = 1; %用于数组下标补偿

kSequence = 0: k_max;
for k = kSequence
%%  得到测量值
    if k == 0
        xSequence(:, k+k_indexC) = x_0;
    else % k > 0 
        xSequence(1, k+k_indexC) = xSequence(1, k-1+k_indexC) + V_c * Delta_t * cos(xSequence(3, k-1+k_indexC));
        xSequence(2, k+k_indexC) = xSequence(2, k-1+k_indexC) + V_c * Delta_t * sin(xSequence(3, k-1+k_indexC));
        xSequence(3, k+k_indexC) = xSequence(3, k-1+k_indexC) + omega_c * Delta_t;
        xSequence(:, k+k_indexC) = xSequence(:, k+k_indexC) + mvnrnd(zeros(3, 1), Q)'; % Add process/dynamicial noise
    end
    %上面的操作主要是得到这里的测量值 xSequence是预测值 ySequence是得到的虚拟的测量值，实际中就是传感器的采样值（GPS）
    ySequence(:, k+k_indexC) = H * xSequence(:, k+k_indexC) + mvnrnd(zeros(3, 1), R)';
    
%%	状态估计
%%	预测步
    if k == 0
        m_k_prior = m_0;%初始期望
        P_k_prior = P_0;%初始方差
    else % k > 0%首先得到预测的雅可比矩阵 主要针对非线性
        F_k_x = [1 0 -V_c*Delta_t*sin(m_kminus1_posterior(3)); 0 1 V_c*Delta_t*cos(m_kminus1_posterior(3)); 0 0 1]; % Jacobian matrix of f_k
        %依次 得到预测的点的期望 这里期望就代表状态，因为这是卡尔曼滤波 
        m_k_prior(1) = m_kminus1_posterior(1) + V_c * Delta_t * cos(m_kminus1_posterior(3));
        m_k_prior(2) = m_kminus1_posterior(2) + V_c * Delta_t * sin(m_kminus1_posterior(3));
        m_k_prior(3) = m_kminus1_posterior(3) + omega_c * Delta_t;
        %依次 得到预测的点的方差 这里期望就代表状态，因为这是卡尔曼滤波 
        P_k_prior = F_k_x * P_kminus1_posterior * F_k_x' + Q;
    end
    
%% 更新步
    %测量残差协方差
    S_k = H * P_k_prior * H' + R;
    %卡尔曼增益
    K_k = P_k_prior * H' / S_k;
    
    m_k_posterior = m_k_prior + K_k * (ySequence(:, k+k_indexC) - H * m_k_prior);%更新的期望 最终状态
    P_k_posterior = P_k_prior - K_k * S_k * K_k';%更新的协方差
    P_k_posterior = (P_k_posterior + P_k_posterior')/2; % 
    x_hatSequence(:, k+k_indexC) = m_k_posterior; % 
    %用于递归
    m_kminus1_posterior = m_k_posterior;
    P_kminus1_posterior = P_k_posterior;
end


%% 图形化表示
tSequence = Delta_t * kSequence;

figure,
plot(tSequence, xSequence(1, :), tSequence, x_hatSequence(1, :), 'LineWidth', 0.5)
xlabel('Time (sec.)')
ylabel('Locations (m)')
title('x^{(1)}-axis')
legend('x^{(1)}', 'Estimate of x^{(1)}')
grid on;

figure,
plot(tSequence, xSequence(2, :), tSequence, x_hatSequence(2, :), 'LineWidth', 0.5)
xlabel('Time (sec.)')
ylabel('Locations (m)')
title('x^{(2)}-axis')
legend('x^{(2)}', 'Estimate of x^{(2)}')
grid on;

figure,
plot(tSequence, xSequence(3, :), tSequence, x_hatSequence(3, :), 'LineWidth', 0.5)
xlabel('Time (sec.)')
ylabel('Angle (rad)')
title('\theta')
legend('\theta', 'Estimate of \theta')
grid on;

figure,
plot(xSequence(1, :), xSequence(2, :), x_hatSequence(1, :), x_hatSequence(2, :), 'LineWidth', 0.5)
xlabel('x^{(1)}-axis (m)')
ylabel('x^{(2)}-axis (m)')
legend('Trajectory', 'Estimate')
grid on;