一、概述
上一篇文章说到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 的思考里,加油去干吧!
评论(1)
您还未登录,请登录后发表或查看评论