之前说了卡尔曼，扩展卡尔曼，两种滤波方式，卡尔曼适用于线性系统，扩展卡尔曼适用于非线性不太强的系统，至于强非线性系统，本文就是为了解决这个问题。

背景

上文讨论的EKF方式，通过对系统方程或者观测方程进行泰勒展开后，仅保留其一阶近似项，这样的做法不可避免的会造成一些误差，如果系统的非线性程度不是很强的话，那还好说，误差可以忽略，强非线性系统的话，这种误差就必须要考虑了；另外，每次都要进行一次雅克比矩阵计算，很多计算平台做偏导计算是不太容易实现的。

基于上面所述的两个缺点，UKF则完全没有这两个问题，不过本质的思想都是一样的，UKF使用无迹变换(Unscented Transform,UT)来处理非线性方程的均值和协方差的传递问题。

UKF的核心思想是对非线性函数的概率密度分布进行近似，用一系列的确定样本来逼近状态的后验概率密度。UKF没有把高阶项忽略，因此UKF有较高的精度。

无迹变换

无迹变换就是在估计点附近进行采样，使用这些样本点表示高斯分布近似状态的概率密度函数。其步骤为：

在原状态分布中，按照规则选取一定的采样点；
将这些采样点带入非线性函数中，得到非线性函数值点集，通过这些点集求取变换后的均值和协方差。

设一个非线性变换y=f(x)y=f(x)y=f(x)，其中状态向量xxx为nnn维随机变量，并且已知其均值为x‾\overline{x}x，协方差为PPP。

上图所示，即为UT变换，下面说一下，如何通过UT变换得到2n+12n+12n+1个SigmaSigmaSigma点χ\chiχ和相应的权重ω\omegaω来计算yyy的统计特征（上标表示为采样点的序列数，下标mmm为均值，下标ccc为协方差）。

第一步，计算当前时刻的SigmaSigmaSigma点，
{χ0=μ,i=0χi=μ+((n+λ)P)i,i=1,…,nχi=μ−((n+λ)P)i,i=n+1,…,2n\begin{cases} \chi^{0}=\mu ,\space i=0 \\ \chi^{i}=\mu+(\sqrt{(n+\lambda)P})_{i}, \space i=1 ,\dots , n \\ \chi^{i}=\mu-(\sqrt{(n+\lambda)P})_{i}, \space i=n+1 ,\dots , 2n \\ \end{cases} ⎩⎪⎨⎪⎧χ0=μ, i=0χi=μ+((n+λ)P)i, i=1,…,nχi=μ−((n+λ)P)i, i=n+1,…,2n

上式中，nnn为系统状态的维度，
λ\lambdaλ为缩放因子，
((n+λ)P)i(\sqrt{(n+\lambda)P})_{i}((n+λ)P)i表示矩阵(n+λ)P(n+\lambda)P(n+λ)P平方根的第iii列（当P=ATAP=A^{T}AP=ATA时，(Pi(\sqrt{P}_{i}(Pi取AAA的第iii行；）。

第二步，计算下一时刻经非线性变换产生的预测采样点，
Yi=f(χi)Y^{i}=f(\chi^{i}) Yi=f(χi)

第三步，计算对应的权重为
{wm0=λn+λwm0=λn+λ+(1−α2+β)wmi=wci=12(n+λ),i=1,…,2n\begin{cases} w^{0}_{m}=\frac{\lambda}{n+\lambda} \\ w^{0}_{m}=\frac{\lambda}{n+\lambda}+(1-\alpha^{2}+\beta) \\ w^{i}_{m}=w^{i}_{c}=\frac{1}{2(n+\lambda)}, \space i=1 ,\dots , 2n \end{cases} ⎩⎪⎨⎪⎧wm0=n+λλwm0=n+λλ+(1−α2+β)wmi=wci=2(n+λ)1, i=1,…,2n

其中，缩放因子λ=α(n+k)−n\lambda=\alpha(n+k)-nλ=α(n+k)−n用来降低总的预测误差，α\alphaα决定了采样点的分布状态，一般取值范围为10−4≤α≤110^{-4} \le \alpha\le 110−4≤α≤1；至于kkk的选取则没有太多的限制，但是起码要保证(n+λ)P(n+\lambda)P(n+λ)P为半正定矩阵，一般都是选取n+k=3n+k=3n+k=3；参数β≥0\beta\ge0β≥0，可以通过调节此项提高方差的精度，把高阶项的影响考虑进来，对于正态分布，选取β=2\beta=2β=2。

第四步，计算非线性变换后的均值和协方差，
{Yˉ=∑i=02nwmiYiPYY=∑i=02nwci[Yi−Yˉ][Yi−Yˉ]T\begin{cases} \bar{Y}=\displaystyle\sum_{i=0}^{2n}w^{i}_{m}Y^{i} \\ P_{YY}=\displaystyle\sum_{i=0}^{2n}w^{i}_{c}[Y^{i}-\bar{Y}][Y^{i}-\bar{Y}]^{T} \end{cases} ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧Yˉ=i=0∑2nwmiYiPYY=i=0∑2nwci[Yi−Yˉ][Yi−Yˉ]T

UKF的实现

由具有高斯白噪声W(k)W(k)W(k)的随机变量XXX和具有高斯白噪声V(k)V(k)V(k)的观测变量ZZZ构成的非线性系统，如下所示，
{X(k+1)=f(X(k),W(k))Z(k)=h(X(k),V(k))\begin{cases} X(k+1)=f(X(k),W(k)) \\ Z(k)=h(X(k),V(k)) \end{cases} {X(k+1)=f(X(k),W(k))Z(k)=h(X(k),V(k))
式中，fff为非线性状态方程函数，hhh为观测方程函数，W(k)W(k)W(k)的协方差矩阵为QQQ，V(k)V(k)V(k)的协方差矩阵为RRR。

具体的UKF计算步骤如下：

计算当前时刻的SigmaSigmaSigma点
Xi(k∣k)=[X^(k∣k)X^(k∣k)+(n+λ)P(k∣k)X^(k∣k)−(n+λ)P(k∣k)](1)X^{i}(k|k)=[\hat{X}(k|k) \hspace{3mm} \hat{X}(k|k)+\sqrt{(n+\lambda)P(k|k)} \hspace{3mm} \hat{X}(k|k)-\sqrt{(n+\lambda)P(k|k)}] \tag{1} Xi(k∣k)=[X^(k∣k)X^(k∣k)+(n+λ)P(k∣k)X^(k∣k)−(n+λ)P(k∣k)](1)
预测当前时刻的SigmaSigmaSigma点集的一步预测
Xi(k+1∣k)=f[k,Xi(k∣k)](2)X^{i}(k+1|k)=f[k,X^{i}(k|k)] \tag{2} Xi(k+1∣k)=f[k,Xi(k∣k)](2)
计算一步预测的均值和协方差矩阵
{X^(k+1∣k)=∑i=02nwiXi(k+1∣k)P(k+1∣k)=∑i=02nwi[X^(k+1∣k)−Xi(k+1∣k)][X^(k+1∣k)−Xi(k+1∣k)]T+Q(3)\begin{cases} \hat{X}(k+1|k)=\displaystyle\sum_{i=0}^{2n}w^{i}X^{i}(k+1|k) \\ P(k+1|k)=\displaystyle\sum_{i=0}^{2n}w^{i}[\hat{X}(k+1|k)-X^{i}(k+1|k)][\hat{X}(k+1|k)-X^{i}(k+1|k)]^{T}+Q \tag{3} \end{cases} ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧X^(k+1∣k)=i=0∑2nwiXi(k+1∣k)P(k+1∣k)=i=0∑2nwi[X^(k+1∣k)−Xi(k+1∣k)][X^(k+1∣k)−Xi(k+1∣k)]T+Q(3)
根据一步预测值，再次使用UT变换，产生新的SigmaSigmaSigma点集
Xi(k+1∣k)=[X^(k+1∣k)X^(k+1∣k)+(n+λ)P(k+1∣k)X^(k+1∣k)−(n+λ)P(k+1∣k)](4)X^{i}(k+1|k)=[\hat{X}(k+1|k) \hspace{2mm} \hat{X}(k+1|k)+\sqrt{(n+\lambda)P(k+1|k)} \hspace{2mm} \hat{X}(k+1|k)-\sqrt{(n+\lambda)P(k+1|k)}] \tag{4} Xi(k+1∣k)=[X^(k+1∣k)X^(k+1∣k)+(n+λ)P(k+1∣k)X^(k+1∣k)−(n+λ)P(k+1∣k)](4)
将k+1k+1k+1时刻的SigmaSigmaSigma点集带入观测方程，得到预测的观测量，
Zi(k+1∣k)=∑i=02nwiZi(k+1∣k)(5)Z^{i}(k+1|k)=\displaystyle\sum_{i=0}^{2n}w^{i}Z^{i}(k+1|k) \tag{5} Zi(k+1∣k)=i=0∑2nwiZi(k+1∣k)(5)
由第五步得到SigmaSigmaSigma点集的观测预测值，通过加权求和得到系统预测的均值以及协方差
{Zˉ(k+1∣k)=∑i=02nwiZi(k+1∣k)Pzkzk=∑i=02nwi[Zi(k+1∣k)−Zˉ(k+1∣k)][Zi(k+1∣k)−Zˉ(k+1∣k)]T+RPxkzk=∑i=02nwi[Xi(k+1∣k)−Zˉ(k+1∣k)][Xi(k+1∣k)−Zˉ(k+1∣k)]T(6)\begin{cases} \bar{Z}(k+1|k)=\displaystyle\sum_{i=0}^{2n}w^{i}Z^{i}(k+1|k) \\ P_{z_{k}z_{k}}=\displaystyle\sum_{i=0}^{2n}w^{i}[Z^{i}(k+1|k)-\bar{Z}(k+1|k)][Z^{i}(k+1|k)-\bar{Z}(k+1|k)]^{T}+R \\ P_{x_{k}z_{k}}=\displaystyle\sum_{i=0}^{2n}w^{i}[X^{i}(k+1|k)-\bar{Z}(k+1|k)][X^{i}(k+1|k)-\bar{Z}(k+1|k)]^{T} \tag{6} \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Zˉ(k+1∣k)=i=0∑2nwiZi(k+1∣k)Pzkzk=i=0∑2nwi[Zi(k+1∣k)−Zˉ(k+1∣k)][Zi(k+1∣k)−Zˉ(k+1∣k)]T+RPxkzk=i=0∑2nwi[Xi(k+1∣k)−Zˉ(k+1∣k)][Xi(k+1∣k)−Zˉ(k+1∣k)]T(6)
计算卡尔曼增益矩阵
K(k+1)=PxkzkPzkzk−1(7)K(k+1)=P_{x_{k}z_{k}}P_{z_{k}z_{k}}^{-1} \tag{7} K(k+1)=PxkzkPzkzk−1(7)
最后，计算系统的状态更新和协方差更新

{X^(k+1∣k+1)=X^(k+1∣k)+K(k+1)[Z(k+1)−Z^(k+1∣k)]P(k+1∣k+1)=P(k+1∣k)−K(k+1)PzkzkK(k+1)T(8)\begin{cases} \hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)[Z(k+1)-\hat{Z}(k+1|k)] \\ P(k+1|k+1)=P(k+1|k)-K(k+1)P_{z_{k}z_{k}}K(k+1)^{T} \tag{8} \end{cases} {X^(k+1∣k+1)=X^(k+1∣k)+K(k+1)[Z(k+1)−Z^(k+1∣k)]P(k+1∣k+1)=P(k+1∣k)−K(k+1)PzkzkK(k+1)T(8)

总结

至此，卡尔曼的三种滤波方式都学完了，线性系统就卡尔曼，非线性不强的系统就使用扩展卡尔曼，强非线性系统无迹卡尔曼。

卡尔曼的原理就不说了，第一篇有很详细的解释。

扩展卡尔曼利用泰勒展开对非线性系统进行拟合，在对展开的系统舍去高阶项，所以不可避免的有一些精度损失，所以不适用于强非线性系统。

无迹卡尔曼的思路则与扩展卡尔曼不同，在估计点附近进行UT变换，使用获得的SigmaSigmaSigma点集的均值和协方差与原统计特征进行匹配，再把SigmaSigmaSigma点集进行非线性映射，以近似得到状态概率密度函数，这种思路本质其实是一种统计近似。

最后，奉上大神链接：

https://blog.csdn.net/qq_37207042/article/details/104062778#t1

https://github.com/suibuliunian/Kalman-and-Bayesian-Filters-in-Python

Apollo学习笔记（19）UKF相关推荐

Ext.Net学习笔记19：Ext.Net FormPanel 简单用法
Ext.Net学习笔记19:Ext.Net FormPanel 简单用法 FormPanel是一个常用的控件,Ext.Net中的FormPanel控件同样具有非常丰富的功能,在接下来的笔记中我们将一起 ...
springmvc学习笔记(19)-RESTful支持
springmvc学习笔记(19)-RESTful支持标签: springmvc springmvc学习笔记19-RESTful支持概念 REST的样例 controller REST方法的前端控 ...
区块链学习笔记19——ETH难度调整
区块链学习笔记19--ETH难度调整学习视频:北京大学肖臻老师<区块链技术与应用> 笔记参考:北京大学肖臻老师<区块链技术与应用>公开课系列笔记--目录导航页前面学过,比特 ...
Apollo学习笔记
Apollo学习笔记 Apollo课程智能驾驶入门课程无人驾驶概览 1.软件层分为三层: 实时操作系统(RTOS):确保在给定时间内完成特定任务,实时时确保系统稳定性.驾驶安全性的重要要求.通过在 ...
Apollo学习笔记进阶课程之三：定位技术②
Apollo学习笔记进阶课程之三:定位技术② 百度的无人驾驶定位方案 1).GNSS定位 GPS误差来源: 上图为单点定位,基于TOA 载波定位技术:(RPK技术,PPP技术) RPK:可以在五秒内 ...
Python学习笔记19：列表 III
Python学习笔记19:列表 III 其实这篇笔记标题应该是列表扩展,从列表开始,将涵盖Python中的序列容器. 关于列表的基础知识,可以看我的前两篇文章: Python学习笔记1:列表. Pyt ...
Linux 学习笔记19 信号
Linux 学习笔记19 信号信号信号概述为什么要是使用信号--为了实现进程的有序退出信号是进程运行过程中,由自身产生或者由进程外部发来的消息.信号是硬件中断的软件模拟(软中断) signal ...
Apollo学习笔记3-定位模块配置
Apollo学习笔记3-定位模块配置环境介绍导航设备参数配置导航设备配置 (1)杆臂配置 (2)GNSS 航向配置 (3)导航模式配置 (4) USB 接口输出设置 (5)网口配置 (6) PP ...
影像组学视频学习笔记(19)-数据标准化、归一化极简概述、Li‘s have a solution and plan.
本笔记来源于B站Up主: 有Li 的影像组学系列教学视频本节(19)主要介绍: 数据的标准化.归一化为什么要进行标准化.归一化? 机器学习算法的要求便于横向比较 # 标准化 (影像组学中最常用) ...
TMS320F280049C 学习笔记19 可配置逻辑块 (CLB) 软件配置
文章目录 CLB tool 简介 CLB配置过程概述软件安装 GNU Compiler Install the Simulation Viewer 使用CLB tool 导入空CLB工程更新变量路 ...

Apollo学习笔记（19）UKF

背景

无迹变换

UKF的实现

总结

Apollo学习笔记（19）UKF相关推荐

最新文章

热门文章