机器人学在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

  • 发表于:
  • 原文链接https://kuaibao.qq.com/s/20181108G1T9JM00?refer=cp_1026
  • 腾讯「云+社区」是腾讯内容开放平台帐号(企鹅号)传播渠道之一,根据《腾讯内容开放平台服务协议》转载发布内容。
  • 如有侵权,请联系 yunjia_community@tencent.com 删除。

扫码关注云+社区

领取腾讯云代金券