点击上方“小白学视觉”，选择加"星标"或“置顶”

重磅干货，第一时间送达

1. 简介

无损卡尔曼滤波又称无迹卡尔曼滤波（Unscented Kalman Filter，UKF），是无损变换（Unscented Transform，UT）与标准卡尔曼滤波体系的结合，通过无损变换变换使非线性系统方程适用于线性假设下的标准卡尔曼体系。

UKF使用的是统计线性化技术，我们把这种线性化的方法叫做无损变换（unscented transformation）这一技术主要通过n个在先验分布中采集的点（我们把它们叫sigma points）的线性回归来线性化随机变量的非线性函数，由于我们考虑的是随机变量的扩展，所以这种线性化要比泰勒级数线性化（EKF所使用的策略）更准确。

和EKF一样，UKF也主要分为预测和更新。

UKF的基本思想是卡尔曼滤波与无损变换，它能有效地克服EKF估计精度低、稳定性差的问题，因为不用忽略高阶项，所以对于非线性分布统计量的计算精度高。

2. CTRV运动模型

恒定转率和速度模型（Constant Turn Rate and Velocity，CTRV）

2.1 CTRV的目标状态量

2.2 CTRV的状态转移函数

2.3 CTRV Process Noise

3. Prediction

分为3个步骤：

• 产生Sigma点

• 预测Sigma点的下一帧状态 （类似于粒子滤波中的预测，更新粒子状态）

• 预测系统状态的均值和方差（类似于粒子滤波中的加权平均）

3.1 Generate Sigma Points

通常，假定状态的个数为 n ，我们会产生 2n+1 个sigma点，其中第一个就是我们当前状态的均值 μ ，sigma点集的均值的计算公式为：

其中的 λ 是一个超参数，根据公式，λ 越大， sigma点就越远离状态的均值，λ 越小， sigma点就越靠近状态的均值。

在我们的CTRV模型中，状态数量 n 除了要包含5个状态以外，还要包含处理噪声 μa 和 μω˙,因为这些处理噪声对模型也有着非线性的影响。在增加了处理噪声的影响以后，我们的不确定性矩阵 P 就变成了：

其中，P′ 就是我们原来的不确定性矩阵（在CTRV模型中就是一个 5×5 的矩阵），Q是处理噪声的协方差矩阵，在CTRV模型中考虑到直线加速度核Q的形式为：

计算增广的Sigma Points

3.2 预测sigma point

3.3 预测均值和方差

x k+1∣k是sigma点集中每个点各个状态量的加权和， P′ 即为先验分布的协方差（不确定性） P k + 1 ∣ k 由每个sigma点的方差的加权和求得。

4. Update

4.1 Predict Measurement

将先验映射到测量空间然后算出均值和方差:

测量分为两个部分，LIDAR测量和RADAR测量，其中LIDAR测量模型本身就是线性的，所以我们重点还是放在RADAR测量模型的处理上面，RADAR的测量f非线性映射函数为：

Measurement model如图所示：

再一次，我们使用无损转换来解决，但是这里，我们可以不用再产生sigma points了，我们可以直接使用预测出来的sigma点集，并且可以忽略掉处理噪声部分。那么对先验的非线性映射就可以表示为如下的sigma point预测（即预测非线性变换以后的均值和协方差）：

这里的 R 也是测量噪声，在这里我们直接将测量噪声的协方差加到测量协方差上是因为该噪声对系统没有非线性影响。在本例中，以RADAR的测量为例，那么测量噪声R为：

4.2 Update State

首先计算出sigma点集在状态空间和测量空间的互相关函数T k + 1 ∣ k T

计算卡尔曼增益K k + 1 ∣

更新状态，计算$x_{k+1|k+1}（其中 z k + 1 是新得到的测量，而 z k + 1 ∣ k 则是我们根据先验计算出来的在测量空间的测量)。

更新状态协方差矩阵，计算P k + 1 ∣ k + 1 

版权声明：本文为CSDN博主「令狐少侠、」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。

原文链接：

https://blog.csdn.net/weixin_42905141/article/details/99710297

下载1：OpenCV-Contrib扩展模块中文版教程

在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。


下载2：Python视觉实战项目52讲
在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。


下载3：OpenCV实战项目20讲
在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。


交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~