相机IMU融合四部曲(二):误差状态四元数详细解读

相机IMU融合四部曲(二):误差状态四元数详细解读

极品巧克力

前言

上一篇文章,《D-LG-EKF详细解读》中,讲了理论上的SE3上相机和IMU融合的思想。但是,还没有涉及到实际的操作,以及实际操作中会遇到的一些问题。所以,本文开始讲实际操作,包括,在相机和IMU融合的过程中,IMU速度的计算,加速度计和陀螺仪的使用,偏移的处理,重力的滤波等。

本文的主要参考文献为John sola的《Quaternion kinematics for the error state Kalman》,简称为误差状态四元数。它的基本思想和D-LG-EKF是一样的,都是对均值状态和扰动状态的进行处理。但是,不同的是,在误差状态四元数里,是把偏移也放到状态里滤波的,而Google Cardboard里的偏移是通过低通滤波滤出来的。

而且John sola的相机和IMU融合的程序是开源的,项目名称为RT-SLAM,源代码地址为https://www.openrobots.org/wiki/rtslam/。

其运行效果如视频(https://vimeo.com/114879173)所示。

本文目标读者:传感器融合算法工程师。

1.预测

首先,列出运动方程。

相机IMU融合四部曲(二):误差状态四元数详细解读_第1张图片

其中,下标t表示的是true的意思。

又因为实际测量值与真实值的关系为,

相机IMU融合四部曲(二):误差状态四元数详细解读_第2张图片

把实际测量值代入到上式中,

相机IMU融合四部曲(二):误差状态四元数详细解读_第3张图片

所以,用D-LG-EKF里面的均值+扰动的思想,表示两个时刻的均值以及扰动状态之间要满足的关系,

因为,,所以,代入上式,得到,

相机IMU融合四部曲(二):误差状态四元数详细解读_第4张图片

里面,其实是,而不是是中间的一个转换。

其中,上式左边的等都为均值,就是由时刻的均值变换过来的,新的均值的计算过程如下,

相机IMU融合四部曲(二):误差状态四元数详细解读_第5张图片

再代回到之前的公式中,

相机IMU融合四部曲(二):误差状态四元数详细解读_第6张图片

从而得到,新的扰动与旧的扰动的关系,

相机IMU融合四部曲(二):误差状态四元数详细解读_第7张图片

的平方项都忽略掉,忽略二阶的极小值,进一步推导。

参考《从角轴到四元数微分方程》,角轴扰动与四元数扰动的关系为,

相机IMU融合四部曲(二):误差状态四元数详细解读_第8张图片

原先的表达式可以转换为,

相机IMU融合四部曲(二):误差状态四元数详细解读_第9张图片

在上面的推导中,因为是二阶极小值,所以忽略掉。因为是各向相同的噪声,所以,参考论文里面的推导,并不会影响协方差的计算。

对下一个表达式进行转换,

相机IMU融合四部曲(二):误差状态四元数详细解读_第10张图片

把上面的四元数乘法,全部都解开,应该就能算出来。但是太麻烦了。

利用,

相机IMU融合四部曲(二):误差状态四元数详细解读_第11张图片

上式转换为,

相机IMU融合四部曲(二):误差状态四元数详细解读_第12张图片

在上式中,因为,认为是一个微小值,所以直接

或者,另外一种方法,参考论文上的方法,对表达式两边求导,导数也应该是相同的。

因为,,所以,

相机IMU融合四部曲(二):误差状态四元数详细解读_第13张图片

在上面,进行了两处简化,首先,忽略掉了项,然后,。积分之后,就得到,

相机IMU融合四部曲(二):误差状态四元数详细解读_第14张图片

与前面那种方法的结果相同。但是,论文上的这种方法难想到,为了思考上的方便,以后仍然还是用前面的那种方法。

启发:这里用旋转矩阵,而不是叉乘,这么操作的目的应该是,叉乘只是对旋转矩阵的近似,而角轴转旋转矩阵,用罗德里格斯变换,得到的结果最准确。所以,最终结果里,还是尽量少用叉乘,能组合成旋转矩阵就组合成旋转矩阵,而角轴到旋转矩阵的方法用罗德里格斯变换。

然后,算后面的扰动。

新的扰动与旧的扰动的关系,总结起来就是,

相机IMU融合四部曲(二):误差状态四元数详细解读_第15张图片

所以,都转换成了线性的关系,可以表示为,

其中,

相机IMU融合四部曲(二):误差状态四元数详细解读_第16张图片

相机IMU融合四部曲(二):误差状态四元数详细解读_第17张图片

因为,,所以,

相机IMU融合四部曲(二):误差状态四元数详细解读_第18张图片

其中,的计算,参考论文上的公式(270)。

相机IMU融合四部曲(二):误差状态四元数详细解读_第19张图片

2.更新

然后,使用贝叶斯公式,用表示,

相机IMU融合四部曲(二):误差状态四元数详细解读_第20张图片

上面的表示的是一种特殊的运算,意思是距离。

为了能像《D-LG-EKF》里面那样转换成卡尔曼滤波的形式,上式的右边内容,需要进行线性化。在扰动的均值处进行线性化,在这里,即为处进行线性化。

当然,,也可以进一步变换,比如,MSF里,把四元数残差转换成角轴残差。但这些残差都要有相对应的

而在误差状态四元数论文里,是通过级联求导的方法。

其中,要根据具体的残差方程来计算,而则是固定的。无论是四元数残差还是角轴残差,还是其它的残差,不同的仅仅只是,而都是一样的。所以,上面的表达式,是一个通用模型,适用于所有的残差,也可以说是,适用于所有的观测。

所以,可以提前计算好,

相机IMU融合四部曲(二):误差状态四元数详细解读_第21张图片

其中,其它项都是单位阵,除了,则计算如下,

相机IMU融合四部曲(二):误差状态四元数详细解读_第22张图片

所以,

这是可以提前计算好的,在实际计算时,只需要把代进来就行。

,则原式可以转换为,

然后,就可以转换为卡尔曼滤波公式,这些对应的是扰动的均值和协方差,

然后,就是把卡尔曼滤波算出来的最大后验值,加入到原先的状态中,

也就是论文里面的reset部分,就是让旧的状态吸收进卡尔曼滤波出来的扰动的均值,让新扰动的均值变为0。

则新的扰动,与旧的扰动的关系为,

相机IMU融合四部曲(二):误差状态四元数详细解读_第23张图片

其中,

相机IMU融合四部曲(二):误差状态四元数详细解读_第24张图片

代回到之前的公式,得到,

相机IMU融合四部曲(二):误差状态四元数详细解读_第25张图片

从而得到,

相机IMU融合四部曲(二):误差状态四元数详细解读_第26张图片

所以,新的扰动的均值为,

相机IMU融合四部曲(二):误差状态四元数详细解读_第27张图片

新的扰动的协方差为,

相机IMU融合四部曲(二):误差状态四元数详细解读_第28张图片

所以,也就得到的新的协方差,

或者,也没必要这么麻烦,直接根据前面新旧扰动的关系,算,然后。其实根据协方差计算公式,本质上是一样的。

3.全局扰动

之前的扰动都是加在右边的,全局扰动就是加在左边的扰动。

全局扰动与本地扰动的区别,如论文中的表格4所示。差别不大,主要是角度上的扰动的雅克比。这里也计算一下。

相机IMU融合四部曲(二):误差状态四元数详细解读_第29张图片

4.微分方程的积分

论文的附录部分,就是讨论用龙格库塔的方法,或者泰勒多阶展开的方法,对状态转移矩阵进行积分。

5.总结

虽然论文中有说这么一句话,全局扰动的方法比局部扰动的方法要好,比如李名杨的MSCKF中的方法,但是没有具体举例说明好在哪里。

用四元数来表示状态,四元数扰动与角轴扰动的转换太麻烦了,可以改成用李代数来表示旋转,但是李代数里面的BCH近似的又不好算。

6.参考文献

  1. Solà J. Quaternion kinematics for the error-state Kalman filter[J]. 2017.

转载于:https://www.cnblogs.com/ilekoaiq/p/9266036.html

你可能感兴趣的:(相机IMU融合四部曲(二):误差状态四元数详细解读)