IMU预积分及残差雅克比计算

最新推荐文章于 2024-05-10 21:06:21 发布

xiaoshuiyisheng

最新推荐文章于 2024-05-10 21:06:21 发布

阅读量6.6k

点赞数 2

分类专栏： IMU VINS

VINS 同时被 2 个专栏收录

5 篇文章 1 订阅

订阅专栏

IMU

4 篇文章 1 订阅

订阅专栏

前段时间推了泡泡机器人邱笑晨博士的IMU预积分公式,收获很大,再看到VIORB的时候,大都用的这一套公式.

后再看VINS的时候,VINS用的是连续时间的预积分,看一些大佬的博客,说代码里写的是离散时间的.这里将所看到的做一下整理.

要求的是:

即要求

这里要求的三个临时状态量只与IMU的偏置有关系，而与其他状态无关可以写成一阶线性展开近似

其中的

论文中用的欧拉积分,代码中用的中值积分,中值积分求法如下:

首先陀螺仪的测量模型和加速度计的测量模型:

离散时间的预积分过程(中值法)

(1) (2) (3)

由泰勒展开式可得

$\delta \theta _{k+1}=\delta \theta _{k}+\dot{\delta \theta _{k}}\delta t$ (4)

由Quaternion kinematics for the error-state KF第54页(237c)公式:

(5)

将(1)代入到(5)得(注意这里 $w_{m}-w_{b}$ 实际上就是 $w_{t}+w_{n}$ 而 $w_{n}$ 在后面的处理,其实就是 $w_{t}$ ):

(6)

将(6)代入到(4)得:

(7)

得到了 $\delta \theta _{k+1}$ 再来看 $\delta v_{k+1}$ .

$\delta \beta _{k+1}=\delta \beta _{k}+\dot{\delta \beta _{k}}\delta t=\delta \beta _{k}+{a}'\delta t$ (8) (注意这里, ${a}'$ 不是a的导数,就是公式3)

由Quaternion kinematics for the error-state KF第54页(237b)公式:

(9)

将(3)代入到(9)可以得:

$\dot{\delta v_{k+1}}=-\frac{1}{2}q_{k}(a_{k}-b_{ak})\delta \theta _{k}-\frac{1}{2}q_{k+1}(a_{k+1}-b_{ak})\delta \theta _{k+1}-\frac{1}{2}q_{k}\delta b_{ak}-\frac{1}{2}q_{k+1}\delta b_{ak}-\frac{1}{2}q_{k}n_{a0}-\frac{1}{2}q_{k}n_{a1}$ (10) 没有 $\delta g$ 项

将(7)代入到(10)可得:

(11)

同理可得到位移的关系:

(12)

然后可以得到陀螺仪和加速度计偏置的误差:

(13)

(14)

综上可以写成矩阵形式:

其中:

矩阵可简写为:

以上所求雅克比为预积分的雅克比

求完

还需要求针对偏置的偏导项,在优化时用到做残差优化.

偏导项部分可以从上面的矩阵形式直接看出.

以上有看完代码后的修改...有很多很糙的截图.

还需要计算残差的雅克比矩阵. 残差的雅克比是用来优化用的.

残差就是将预积分相当于测量值（就是真值，因为没有比这个更准确的值了，那当然就是真值了），要估计的状态就是估计值，所以预积分测量值减去状态估计值就是残差，在后面会提到需要估计的IMU估计值有p,v,q,ba,bg。P和q的估计值初始值比较好得到（和视觉相关，可以直接用视觉的初值），而v,ba,bg这三个量的估计值初始值就比较难得到了，因为视觉没有这三个初始量，就会用到下面的联合初始化得到初始的这三个量.

残差向量 $r_{\Delta R_{ij}}, r_{\Delta v_{ij}}, r_{\Delta P_{ij}}$ ,状态向量和偏置 $\delta b_{a_{k}},\delta b_{a_{k+1}},\delta b_{w_{k}},\delta b_{w_{k+1}}$ .

残差可以表示为:

首先看 $r_{\Delta R_{ij}}$

1. 对Ri求偏导.

2. 对Rj求偏导.

$2\left [ q_{b_{k+1}}^{w}^{-1}\otimes \left [ 0,I/2 \right ]^{T}\otimes q_{b_{k}}^{w}\otimes \check{r}_{b_{k+1}}^{b_{k}} \right ]$

3. 对偏置 $\delta b_{w_{k}}$ 求偏导.

$2\left [ q_{b_{k+1}}^{w}^{-1}\otimes q_{b_{k}}^{w}\otimes \check{r}_{b_{k+1}}^{b_{k}} \otimes\left [ 0,J_{b_{w}}^{r}/2 \right ]^{T} \right ]$

然后是对 $r_{\Delta v_{ij}}$ 求雅克比

1. 对 $\delta b_{w_{k}}$ 求偏导.

$-J_{b_{w}}^{\beta }$

2. 对 $\delta b_{a_{k}}$ 求偏导.

$-J_{b_{a}}^{\beta }$

3. 对 $v_{b_{k}}$ 求偏导.

4. 对 $v_{b_{k+1}}$ 求偏导.

$q_{w}^{b_{k}}$

5. 对Ri求偏导.

最后 $r_{\Delta R_{ij}}$ 求雅克比

1. 对 $\delta b_{w_{k}}$ 求偏导

$-J_{b_{w}}^{P}$

2. 对 $\delta b_{a_{k}}$ 求偏导

$-J_{b_{a}}^{P}$

3. 对 $P_{b_{k}}$ 求偏导

4. 对 $P_{b_{k+1}}$ 求偏导

5. 对 $v_{b_{k}}$ 求偏导

6. 对Ri求偏导

全部的残差雅克比求完了.

参考: 五行缺帅wangshuailpp博客

VINS-Mono代码分析总结

邱笑晨博士的IMU预积分推导

游振兴大佬的四元数运动学笔记（1）旋转的表示

xiaoshuiyisheng

关注

2
点赞
踩
25

收藏

觉得还不错? 一键收藏
6
评论
IMU预积分及残差雅克比计算

前段时间推了泡泡机器人邱笑晨博士的IMU预积分公式,收获很大,再看到VIORB的时候,大都用的这一套公式. 后再看VINS的时候,VINS用的是连续时间的预积分,看一些大佬的博客,说代码里写的是离散时间的.这里将所看到的做一下整理. 要求的是: 即要求这里要求的三个临时状态量只与IMU的...
复制链接

扫一扫