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

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

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

  要求的是:

                    

即要求

            

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

      

  其中的

             

  ( 个人理解:相当于笑晨博士预积分推导中的以下公式

              中

                          这一部分.)

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

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

    

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

        (1)  (2)  (3)

                  由泰勒展开式可得 

                                             (4)

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

                                  (5)

                 将(1)代入到(5)得(注意这里实际上就是而在后面的处理,其实就是):

                                (6)

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

                           (7)

                  得到了再来看.

                          (8)  (注意这里,不是a的导数,就是公式3)

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

                             (9)

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

                        (10)  没有项

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

                        (11)

                     同理可得到位移的关系:

                (12)

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

                                  (13)

                                  (14)

                            综上可以写成矩阵形式:

                  

                      其中:

                       

                矩阵可简写为:

             

              

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

           求完

                 

    还需要求针对偏置的偏导项,加起来才是真正要求的.

          

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

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

 

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

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

             残差向量 ,状态向量和偏置.

             残差可以表示为:

             

           首先看

1.    对Ri求偏导.

         

2. 对Rj求偏导.

        

3. 对偏置求偏导.

      

     然后是对求雅克比

   1.   对求偏导.

            

   2.   对求偏导.

            

    3.   对求偏导.

         

    4.   对求偏导.

          

     5.   对Ri求偏导.

         

              

     最后求雅克比

   1.  对 求偏导

        

   2. 对求偏导

       

    3. 对求偏导

       

    4. 对求偏导

        

     5. 对求偏导

         

      6. 对Ri求偏导

          

       

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

参考: 五行缺帅wangshuailpp博客

          VINS-Mono代码分析总结

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

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