机器人学在Python中的实现（2）：UKF

hello，大家好！我是MPIG2018级研究生刁金辉。今天给大家带来的是无迹卡尔曼滤波相关内容的介绍。在上一章节中介绍了扩展卡尔曼，它主要是计算函数f和h的雅可比矩阵，但是实际操作起来非常困难，特别是对于一些复杂的非线性系统。因此往往采用泰勒展开去一阶线性的部分。由于近似得到的卡尔曼在增益也就不是最优卡尔曼增益，而是近似最优卡尔曼增益。所以今天介绍的UKF则避免了求取函数的雅可比矩阵，从而提高了滤波器的性能。

首先我们看下今天的主要内容：

1. 卡尔曼滤波相关原理

2. 无迹卡尔曼滤波相关原理

3. 无迹卡尔曼算法以及基于Python的代码

4. 无迹卡尔曼相关的例子

（一）卡尔曼滤波相关原理

卡尔曼滤波的核心思想：根据当前的仪器“测量值”和上一刻的“预测量”和“误差”计算得到当前的最优量，再在预测下一刻的量。以k-1时刻的最优估计X(k-1)为准，预测k时刻的状态变量X(k/k-1)，同时又对该状态进行观测，得到观测变量Z(k)，再在预测与观测之间分析，或者说以观测量对预测量进行修订，从而得到k时刻的最优状态。

卡尔曼滤波的核心算法：

（二）无迹卡尔曼滤波相关原理

无迹卡尔曼：无损变换(UT) 和标准Kalman滤波体系的结合，UKF依然没有脱离KF的框架。只不过对下一时刻状态的预测方法变成了sigma点集的扩充与非线性映射。

无损变换(UT)：

1. 首先选择一组点，称为sigma点

2.然后通过非线性函数映射这些点

3.最后通过对映射点计算高斯分布形式，和每个sigma点的权重

（三）无迹卡尔曼算法以及基于Python的代码

1 预测过程：Xk-1|k-1 PX-1|X-1

预测过程基于Python的的代码实现：

2 更新过程：Xk|k-1 PX|X-1

更新过程基于python的代码实现：

3. 模拟的结果

绿色是定位观测点，蓝色是真实轨迹，黑色是航位推算轨迹，红色是最佳运行轨迹。

UKF的总体的估计过程：

1. Sigma点采样;

2. 利用状态方程传递采样点;

3. 利用预测采样点以及均值，计算预测均值以及协方差

4. 预测测量采样点

5. 预测测量值和协方差

6. 计算UKF的增益更新状态向量和方差

（四）无迹卡尔曼相关的例子

前面我们介绍了关于无迹滤波器相关的原理与算法，也运用了非线性变换讲了关于ukf对预测方向的定位，为了更加认识的UKF，下面的例子带大家看看：

UKF Joint Estimation With Neural Network

预测过程：

更新过程：

模拟结果：

随着Round的增加，可以看到误差的变化情况。

想要更加详细了解本讲更多细节的内容吗？那就一起来观看下面的Presentation的具体讲解吧：

Github上代码以及UKF相关例子的详细说明下载：

https://github.com/MPIG-Robot/robotics_in_python