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

专栏文章内容及配图由作者撰写发布，仅供工程师学习之用，如有侵权或者其他违规问题，请联系本站处理。 联系我们