专栏名称: 泡泡机器人SLAM

SLAM的最新资讯，干货内容。

【泡泡机器人好书一起读】机器人kalman教程第3课扩展Kalman滤波及多传感器融合

泡泡机器人SLAM · 公众号 · 机器人 · 2017-04-27 21:02

正文

1 概述

你一直在制作的钢铁侠系统遇到了一个棘手的问题，你有多个返回相同状态变量的传感器，你经常听说 kalman 滤波融合，可是怎么利用 kalman 进行多传感器的融合呢，之前你了解的 kalman 知识中并没有涉及。不要着急，这节课将会详细介绍给你。

2 多源信息融合实例——KF

为了保证在任意高度时刚铁侠系统传感器返回数据具有鲁棒性，你选择了 GPS 和气压计两款传感器用来返回观察到了高度信息。

所以对于的感测方程，有如下的观测模型：

再返回到第一节课的内容，依然按照线性kalman滤波的方程建立算法程序（对照第一节课的5个方程后以下程序），进行测试后得到了很好的传感器融合的效果。

所以线性传感器的多个融合可以通过这个简单的方法，这就是滤波融合的关键，是不是很简单，关键程序如下 ( 假设 , 每个传感器个接受 500 个观测值 )

2.1 伪代码

xhat_signal[0] = (z_signal[0][0] +z_signal[1][0]) / 2;

p = 1;

for (k = 1; k < 1000; k++)

{

xhat_signal[k] = xhat_signal[k-1];

p += q;

a = p + r1;

d = p + r2;

e = p / (a*d-p*p);

g1 = e*d - e*p;

g2 = e*a -e*p;

p = (1 - (g1+g2)) * p;

xhat_signal[k] = xhat_signal[k] +g1*(z_signal[0][k]-

xhat_signal[k]) + g2*(z_signal[1][k]-xhat_signal[k]);

}

2.2 滤波过程

怎么样，再对照下第一课的公式内容理解下：

现在你可以很好的解决线性多传感器融合的问题了吧，传感器融合是 kalman 滤波应用的一个重要领域。在网上搜索 kalman 滤波会有一个经典的案例 — 加速度计和角速度计融合，很多人不能很好的理解，因为设计到 EKF 的知识，在本节课的后面会给大家重点讲解一下，首先让我们继续学习下什么是 EKF 。

3 EKF理论

3.1 EKF概述

在上一节课中，你和 Dsy 一同推导了动态系统的估计方法，但是实际系统中有诸多的非线性部分。怎么建立非线性系统的滤波算法，前辈们使用简单的思路将线性 kalman 扩展到非线性的领域，这种常规的方法被成为EKF（扩展kalman滤波）。

扩展 kalman 滤波的原理很简单，就是利用泰勒公式将非线性的部分展开成高阶，并去掉高阶项，利用这个线性化技巧将其转化为一个近似的线性滤波问题。

EKF 的优点是不必预先计算标称轨迹，但是它只能在滤波误差及预测误差较小时才能使用。

正如介绍的一样， EKF 是基于线性 kalman 的，同样将第一节课介绍的公式改进如下：

和是非线性部分，为了方便大家理解，这里去掉了状态转移矩阵。

3.2 EKF应用实例

包含状态转移矩阵的 EKF 又是怎样的呢，为了说明 EKF 的问题，我来举个简单的标量系统进行分析。系统的状态方程如果是：

观测方程为：

首先要将状态方程一阶线性化，求解状态转移矩阵。

然后一阶线性化观测矩阵 H(k):

同样，如果状态方程和感测矩阵不是一维的，就要用雅克比矩阵来线性化，其实原理是一样的，雅克比矩阵只不过是线性代数中的一种多元函数求导的方法。

3.3 EKF滤波过程

总结下 EKF 的处理过程可以归纳为以下几步：

第一步：一阶线性化状态方程，求解状态转移矩阵；

第二步：阶线性化观测矩阵，求解观测矩阵 C(k) ；

第三步：求协方差矩阵预测 P(k|k-1) ：

第四步：求 Kalman 滤波增益：

第五步：求状态更新：

第六步：协方差更新：

以上六步为 EKF 的一个计算周期，也是最经典的 EKF 处理过程。

将公式直接转换成程序就实现了 EKF 的应用，你依然想了解其中的数学推演，这部分内容其实很简单，最关键的推演核心在于通过已知的概率密度函数P(X,Kt) ，获得一个实际的估计算法， EKF 的方案就是通过泰勒级数展开公式线性化获得当前状态向量的估计。 Dsy 告诉你， EKF 总结成一句话就是通过把你的非线性控制模型在雅克比空间的小范围内线性化，然后再利用线性卡尔曼滤波的方法进行滤波。 所以这部分数学方法的推演其实和线性 kalman 的内容很相似，看一下前几节课的内容自行推导吧，很简单。