扩展卡尔曼滤波器在雷达上的应用

简介

对激光雷达和毫米波雷达的测量值进行扩展卡尔曼滤波器跟踪可以减少传感器测量的噪声/误差，并提供对跟踪对象位置的稳健估计。

蓝色的汽车是被跟踪的对象，但是被跟踪的对象可以是任何类型，例如：行人、车辆或其他移动物体。我们不断获得汽车在定义坐标中位置的激光雷达（红圈）和雷达（蓝圈）测量结果，但数据中可能存在噪声和误差。此外，我们需要找到一种方法来融合两种类型的传感器测量值，以估计被跟踪物体的正确位置。因此，我们使用扩展卡尔曼滤波器来计算蓝色汽车的估计位置（绿色三角形）。将估计轨迹（绿色三角形）与蓝色汽车的地面真实轨迹进行比较，并以 RMSE 格式实时显示误差。

扩展卡尔曼滤波器数据处理框图：

扩展卡尔曼滤波器和传统卡尔曼滤波器对比：

传统卡尔曼滤波器：

x 是平均状态向量。

F是状态转移函数。

P是状态协方差矩阵，表示对象状态的不确定性。

u 是过程噪声，它是均值为零、协方差为 Q 的高斯分布。

Q 是过程噪声的协方差矩阵。

y 是创新项，即测量与预测之间的差异。为了计算创新项，我们通过测量函数将状态转换到测量空间，以便我们可以直接比较测量和预测。

S是预测的测量协方差矩阵，或称为新息协方差矩阵。

H 是测量函数。

z 是测量值。

R 是测量噪声的协方差矩阵。

I 是单位矩阵。

K 是卡尔曼滤波器增益。

Hj 和 Fj 是雅可比矩阵。

扩展卡尔曼滤波器：

为了计算预测状态向量 x'，使用预测函数f(x)代替F矩阵。计算P′时，F矩阵将被Fj（f的jocobian矩阵）代替。为了计算创新 y，使用测量函数 h(x') 代替 H 矩阵。在计算S、K、P时，卡尔曼滤波器中的H矩阵将被Hj（h(x')的雅可比矩阵）替代。

所有卡尔曼滤波器都具有相同的三个步骤： 初始化 预测 更新

标准卡尔曼滤波器只能处理线性方程。扩展卡尔曼滤波器（EKF）和无迹卡尔曼滤波器（UKF ）都允许您使用非线性方程； EKF 和 UKF 之间的区别在于它们处理非线性方程的方式：扩展卡尔曼滤波器使用雅可比矩阵来线性化非线性函数；另一方面，无迹卡尔曼滤波器不需要对非线性函数进行线性化，而是从高斯分布中获取代表点。

主要实现源码：

篇幅有限仅展示部分源码截图，获取完整源码请戳下方按钮