一、概述

上一篇文章说到kalman滤波就是用预测的位姿(一般imu提供)和观测的位姿(一般由gps或者雷达和点云地图匹配得到)进行加权融合,并针对kalman的五个公式说明了整个加权过程,但这只是理解,要真正实现一整套融合系统,就需要把刚才那简单的几句话再展开成具体的实现步骤,做到这一点,除了kalman以外,还需要用到更早之前介绍的imu解算、imu误差模型等知识,如果已经忘了,还是需要翻一翻前面文章复习一下的,不然不好理解啦。

二、ESKF 模型

在介绍流程之前,还是要把kalman的模型给列出来,所谓kalman的模型,就是指kalman五个公式中的各个矩阵里面都是啥。

我们以IMU做预测,雷达和点云地图匹配(匹配得到位置和姿态)做观测为例,来展开模型(系统模型如下图)。在这种融合系统中,虽然我们最终想要的融合结果是位置、速度、姿态,但是实际在kalman中作为状态量的是他们的误差值,这种以误差作为状态量的滤波方法较多ESKF(Error State Kalman Filter)。之所以使用误差状态,是因为它有比直接使用位姿作为状态的方法(一般称这种为名义状态)更多的优势,具体这个优势是啥,这个说来话长,不在此展开了,先把融合系统搞明白再说。

1.状态方程

状态方程由误差方程得来,误差方程在前面已经完成推导,此处只再列出结论:

[公式]

[公式]

[公式]

[公式] 

 [公式]

[公式] 

 [公式]

[公式]  [公式]

则误差方程可以写成状态方程的通用形式:

[公式]

其中

[公式]

[公式]

[公式]

注:当选择 

[公式] 

 [公式] 时,矩阵形式不一样,请各位自行推导。

2.观测方程

在滤波器中,观测方程一般写为

[公式]

此例中观测量有位置、姿态误差,则

[公式]

因此有

[公式]

[公式]

而此处为 n 观测噪声

[公式]

观测量中, δp 的计算过程为:

[公式]

其中

 [公式] 为 IMU 解算的位置,即预测值。p为雷达与地图匹配得到的位置,即观测值。

[公式] 的计算过程稍微复杂,需要先计算误差矩阵

[公式]

其中

 [公式] 为 IMU 解算的旋转矩阵,即预测值。

 [公式] 为雷达与地图匹配得到的旋转矩阵,即观测值。

由于预测值与观测值之间的关系为

[公式]

因此

[公式]

3. 构建滤波器

构建滤波器,即把融合系统的状态方程和观测方程应用到 Kalman 滤波的五个公式中。前面推导的方程是连续时间的,要应用于离散时间,需要按照采样时间对其进行离散化。

状态方程离散化,可以写为

[公式]

其中

[公式]

[公式]

这里T 为 Kalman 的滤波周期。需要注意的是,关于

 [公式] 的离散化形式,不同资料有差异,但对实际调试影响不大。对于观测方程,不需要乘以滤波周期,可以直接写出

[公式]

将以上各变量,带入kalman滤波的五个方程,即可构建完整的滤波器更新流程。

[公式]

三、融合流程

融合流程倒算是步骤比较多的,但是弄清它的思路,把步骤一个一个列出来,也能做到挺清晰。先给一张图吧,后面的步骤展开就按这张图来讲,图里面的序号与后面的步骤序号一一对应。

1.位姿初始化

在点云地图中实现初始定位,并给以下变量赋值

[公式] : 初始时刻位置

[公式] : 初始时刻速度(可以从组合导航获得)

[公式] : 初始时刻姿态(也可用四元数,后面不再强调)

2.Kalman初始化

1)状态量

[公式]

2)方差

[公式]

初始方差理论上可设置为各变量噪声的平方,实际中一般故意设置大一些,这样可加快收敛速度。

3)过程噪声与观测噪声

[公式]

[公式]

过程噪声与观测噪声一般在 kalman 迭代过程中保持不变。

3.惯性解算

按照之前讲解的惯性解算方法,进行位姿更新,该位姿属于预测位姿。

1)姿态解算

[公式]

其中

[公式]

[公式]

[公式]

2)速度解算

[公式]

其中

[公式]

[公式]

3)位置解算

[公式]

4.Kalman预测更新

执行kalman五个步骤中的前两步,即

[公式]

[公式]

当然,这需要先根据公式计算

[公式] 

 [公式] 

5.无观测时后验更新

无观测时,不需要执行kalman剩下的三个步骤,后验等于先验,即

[公式]

6.有观测时的量测更新

执行kalman滤波后面的三个步骤,得到后验状态量

[公式]

7.有观测时计算后验位姿

根据后验状态量,更新后验位姿。

[公式]

8.有观测时状态量清零

状态量已经用来补偿,因此需要清零

[公式]

后验方差保持不变.

9.输出位姿

把后验位姿输出给其他模块使用,即输出

 [公式] 

 [公式]  

[公式] (或

 [公式] )。

四、总结

其实也没啥可总结的,流程步骤虽然多,但还算清晰。

剩下的就是在实际系统中去更加深刻地去理解它的,kalman的内涵是丰富的,但这个丰富不在理论方面,而是在层出不穷的 badcase 以及解决 badcase 的思考里,加油去干吧!