VINS-Mono+Fusion源码解析系列（十八）：IMU预积分约束

1. 图优化

（1）使用方法

​ 损失函数$F(x)$由残差函数$f(x)$​组成：
$$\underset{x}{\min }F(x)=\frac{1}{2}||f(x)|{|}_2^2$$
​ 当考虑方差时，可以写为：
$$\underset{x}{\min }F(x)=f(x{)}^T\Omega f(x)$$
​ 利用高斯牛顿方法进行优化求解，可得到如下增量方程：
$$\begin{gathered} \underbrace{J^T\Omega J}\Delta x=\underbrace{-J^T\Omega f(x)} \\ Hg \end{gathered}$$
（2）优化中使用预积分的步骤

​ 通过上述增量方程可知，需要获取的变量如下：

• 待优化变量的残差$\Delta x$
• 残差关于待优化变量的雅可比$J$
• 残差的方差$\Omega$
• 在预积分优化中，bias也作为待优化变量参与优化，为避免因bias变化而重新进行预积分，需要在bias处通过泰勒展开进行更新

2. IMU预积分约束

2.1 IMU预积分残差

（1）残差定义

​ 已知姿态更新方法如下：
$${\left[\begin{array}{c}{p}_{w{b}_j}\\ q_{w{b}_j}\\ v_j^w\\ b_j^a\\ b_j^g\end{array}\right]}_{15\times 1}=\left[\begin{array}{c}{p}_{w{b}_i}+v_i^w\Delta t-\frac{1}{2}{g}^w\Delta t^2+q_{w{b}_i}{\alpha }_{b_i{b}_j}\\ q_{w{b}_i}{q}_{b_i{b}_j}\\ v_i^w-g^w\Delta t+q_{w{b}_i}{\beta }_{b_i{b}_j}\\ b_i^a\\ b_i^g\end{array}\right]$$
​ 于是，残差可以写为如下形式：
$$e_B(x_i,x_j)={\left[\begin{array}{c}{r}_p\\ r_q\\ r_v\\ r_{ba}\\ r_{bg}\end{array}\right]}_{15\times 1}=\left[\begin{array}{c}{q}_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}\\ 2[q_{b_i{b}_j}^{-1}\otimes (q_{w{b}_i}^{-1}\otimes q_{w{b}_j}){]}_{xyz}\\ q_{w{b}_i}^{-1}(v_j^w-v_i^w+g^w\Delta t)-{\beta }_{b_i{b}_j}\\ b_j^a-b_i^a\\ b_j^g-b_i^g\end{array}\right]$$
​ 待优化变量为：$$\left[\begin{array}{ccccc}{p}_{w{b}_i}& q_{w{b}_i}& v_i^w& b_i^a& b_i^g\end{array}\right]$$$$\left[\begin{array}{ccccc}{p}_{w{b}_j}& q_{w{b}_j}& v_j^w& b_j^a& b_j^g\end{array}\right]$$

​ 在求雅可比时，是利用残差对待优化变量的扰动来求导得到的：
$$\begin{gathered} \left[\begin{array}{ccccc}\delta p_{w{b}_i}& \delta q_{w{b}_i}& \delta v_i^w& \delta b_i^a& \delta b_i^g\end{array}\right] \\ \left[\begin{array}{ccccc}\delta p_{w{b}_j}& \delta q_{w{b}_j}& \delta v_j^w& \delta b_j^a& \delta b_j^g\end{array}\right] \end{gathered}$$

（2）代码实现

​ 主要是计算IMU中位姿，速度以及零偏的相邻时刻的残差，在计算时需要考虑零偏变化对IMU预积分的影响，因此需要先更新IMU预积分（在零偏处进行一阶泰勒展开）

// 计算和给定相邻帧状态量的残差
    Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, 
                                          const Eigen::Quaterniond &Qi, 
                                          const Eigen::Vector3d &Vi, 
                                          const Eigen::Vector3d &Bai, 
                                          const Eigen::Vector3d &Bgi,
                                          const Eigen::Vector3d &Pj, 
                                          const Eigen::Quaterniond &Qj, 
                                          const Eigen::Vector3d &Vj, 
                                          const Eigen::Vector3d &Baj, 
                                          const Eigen::Vector3d &Bgj)
    {
        Eigen::Matrix<double, 15, 1> residuals;  // 定义15维的残差项
        // 下面的雅可比在预积分过程中已求解出
        // 位移预积分在加速度零偏ba处的导数(一阶雅可比)
        Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
        // 位移预积分在陀螺仪零偏bg处的导数(一阶雅可比)
        Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
		// 姿态预积分在加速度零偏ba处的导数(一阶雅可比)
        Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
		// 速度预积分在加速度零偏ba处的导数(一阶雅可比)
        Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA); 
        // 速度预积分在陀螺仪零偏bg处的导数(一阶雅可比)
        Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);  

        Eigen::Vector3d dba = Bai - linearized_ba;  // 加速度零偏ba变化量
        Eigen::Vector3d dbg = Bgi - linearized_bg;  // 角速度零偏bg变化量

/*
   delta_q, delta_v, delta_p为原先的预积分量
   corrected_delta_q, corrected_delta_v, corrected_delta_p为考虑相应零偏变化的预积分量
   因为零偏也是待优化变量，为了避免因零偏变化导致重新进行预积分，对原先预积分量在相应零偏处进行一阶泰勒展开
   对姿态预积分量在零偏处(dbg)进行一阶泰勒展开(对应的雅可比为dp_dbg), 得到考虑零偏变化的新的姿态预积分量
*/
        Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
        // 对速度预积分量在零偏处(dba和dbg)进行一阶泰勒展开(对应的雅可比分别为dp_dba和dp_dbg), 得到考虑零偏变化的新的速度预积分量
        Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
        // 对平移预积分量在零偏处(dba和dbg)进行一阶泰勒展开(对应的雅可比分别为dp_dba和dp_dbg), 得到考虑零偏变化的新的平移预积分量
        Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

/*
   Qi:q_{b_i}^w   Qi:q_{b_j}^w   G: g^w  sum_dt:delta_t   
   Pj:p_{b_j}^w   Pi:p_{b_i}^w   Vi:v_{b_i}^w   Vj:v_{b_j}^w   
   corrected_delta_p:alpha_{b_j}^{b_i}   corrected_delta_q: q_{b_j}^{b_i}   
   corrected_delta_v: beta_{b_j}^{b_i}
*/
        // 位置残差项
        residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
        // 姿态残差项
        residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();  // vec(): 取四元数的虚部
        // 速度残差项
        residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
        // 零偏残差项  直接相减
        residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
        residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
        return residuals;
    }

2.2 IMU残差对待优化变量的雅可比

2.2.1 残差对i时刻各状态量的雅可比

（1）位移残差在$i$时刻对各状态量的雅可比

• $i$时刻位移残差$r_p$对位移的雅可比

$$\frac{\partial r_p}{\partial \delta p_i{p}_i^{,}}=\frac{\partial [q_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}]}{\partial \delta p_i{p}_i^{,}}=\frac{\partial q_{w{b}_i}^{-1}(-p_{w{b}_i})}{\partial \delta p_i{p}_i^{,}}=-q_{w{b}_i}^{-1}$$

• $i$时刻位移$r_p$对姿态的雅可比
  
• $i$时刻位移$r_p$对速度的雅可比

$$\frac{\partial r_p}{\partial \delta v_i^w}=\frac{\partial [q_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}]}{\partial \delta v_i^w}=\frac{\partial q_{w{b}_i}^{-1}(-v_i^w\Delta t)}{\partial \delta v_i^w}=-q_{w{b}_i}^{-1}\Delta t$$

• $i$时刻位移$r_p$对加速度计偏置的雅可比

$$\begin{gathered} \frac{\partial r_p}{\partial \delta b_i^a}=\frac{\partial [q_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}]}{\partial \delta b_i^a} \\ =\frac{\partial (-{\alpha }_{b_i{b}_j})}{\partial \delta b_i^a}=\frac{\partial (-{\alpha }_{b_i{b}_j}-J_{b_a}^{\alpha }\delta b_a-J_{b_g}^{\alpha }\delta b_g)}{\partial \delta b_i^a}=-J_{b_a}^{\alpha } \end{gathered}$$

• $i$时刻位移$$r_p$对陀螺仪偏置的雅可比

$$\begin{gathered} \frac{\partial r_p}{\partial \delta b_i^g}=\frac{\partial [q_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}]}{\partial \delta b_i^g} \\ =\frac{\partial (-{\alpha }_{b_i{b}_j})}{\partial \delta b_i^g}=\frac{\partial (-{\alpha }_{b_i{b}_j}-J_{b_a}^{\alpha }\delta b_a-J_{b_g}^{\alpha }\delta b_g)}{\partial \delta b_i^g}=-J_{b_g}^{\alpha } \end{gathered}$$

（2）姿态残差在$i$时刻对各状态量的雅可比

• $i$时刻姿态残差$r_q$对位移的雅可比

$$\frac{\partial r_q}{\partial \delta p_i{p}_i^{,}}=\frac{\partial 2[q_{b_i{b}_j}^{-1}\otimes (q_{w{b}_i}^{-1}\otimes q_{w{b}_j}){]}_{xyz}}{\partial \delta p_i{p}_i^{,}}=0$$

• $i$时刻姿态残差$r_q$对姿态的雅可比
  
• $i$时刻姿态残差$r_q$对速度的雅可比

$$\frac{\partial r_q}{\partial \delta v_i^w}=\frac{\partial 2[q_{b_i{b}_j}^{-1}\otimes (q_w{b_i}^{-1}\otimes q_{w{b}_j}){]}_{xyz}}{\partial \delta v_i^w}=0$$

• $i$时刻姿态残差$r_q$对加速度计偏置的雅可比

$$\frac{\partial r_q}{\partial \delta b_i^a}=\frac{\partial 2[q_{b_i{b}_j}^{-1}\otimes (q_{w{b}_i}^{-1}\otimes q_{w{b}_j}){]}_{xyz}}{\partial \delta b_i^a}=0$$

• $i$时刻姿态残差$r_q$对陀螺仪偏置的雅可比
  （3）速度残差在$i$时刻对各状态量的雅可比

• $i$时刻速度残差$r_v$对位移的雅可比

$$\frac{\partial r_v}{\partial \delta p_{b_i{b}_i^{,}}}=\frac{\partial q_{w{b}_i}^{-1}(v_j^w-v_i^w+g^w\Delta t)-{\beta }_{b_i{b}_j}}{\partial \delta p_{b_i{b}_i^{,}}}=0$$

• $i$时刻速度残差$r_v$对姿态的雅可比
  
• $i$时刻速度残差$r_v$对速度的雅可比

$$\frac{\partial r_v}{\partial \delta v_i^w}=\frac{\partial [q_{w{b}_i}^{-1}(v_j^w-v_i^w+g^w\Delta t)-{\beta }_{b_i{b}_j}]}{\partial \delta v_i^w}=-q_{w{b}_i}^{-1}$$

• $i$时刻速度残差$r_v$对加速度计偏置的雅可比

$$\begin{gathered} \frac{\partial r_v}{\partial \delta b_i^a}=\frac{\partial [q_{w{b}_i}^{-1}(v_j^w-v_i^w+g^w\Delta t)-{\beta }_{b_i{b}_j}]}{\partial \delta b_i^a} \\ =\frac{\partial (-{\beta }_{b_i{b}_j})}{\partial \delta b_i^a}=\frac{\partial (-{\beta }_{b_i{b}_j}-J_{b_a}^{\beta }\delta b_a-J_{b_g}^{\beta }\delta b_g)}{\partial \delta b_i^a}=-J_{b_a}^{\beta } \end{gathered}$$

• $i$时刻速度残差$r_v$对陀螺仪偏置的雅可比
  
  （4）加速度计偏置残差在$i$时刻对各状态量的雅可比

• $i$时刻加速度计偏置残差$r_{b_a}$对位移的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta p_i{p}_i^{,}}=\frac{\partial (b_j^a-b_i^a)}{\partial \partial \delta p_i{p}_i^{,}}=0$$

• $i$时刻加速度计偏置残差$r_{b_a}$对姿态的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta {\theta }_{b_i{b}_i^{,}}}=\frac{\partial (b_j^a-b_i^a)}{\partial \delta {\theta }_{b_i{b}_i^{,}}}=0$$

• $i$时刻加速度计偏置残差$r_{b_a}$对速度的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta v_i^w}=\frac{\partial (b_j^a-b_i^a)}{\partial \delta v_i^w}=0$$

• $i$时刻加速度计偏置残差$r_{b_a}$对加速度计偏置的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta b_i^a}=\frac{\partial (b_j^a-b_i^a)}{\partial \delta b_i^a}=-I$$

• $i$时刻加速度计偏置残差$r_{b_a}$对陀螺仪偏置的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta b_i^g}=\frac{\partial (b_j^a-b_i^a)}{\partial \delta b_i^g}=0$$

（5）陀螺仪偏置残差在$i$时刻对各状态量的雅可比

• $i$时刻陀螺仪偏置残差$r_{b_g}$对位移的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta p_i{p}_i^{,}}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta p_i{p}_i^{,}}=0$$

• $i$时刻陀螺仪偏置残差$r_{b_g}$对姿态的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta {\theta }_{b_i{b}_i^{,}}}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta {\theta }_{b_i{b}_i^{,}}}=0$$

• $i$时刻陀螺仪偏置残差$r_{b_g}$对速度的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta v_i^w}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta v_i^w}=0$$

• $i$时刻陀螺仪偏置残差$r_{b_g}$对加速度计偏置的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta b_i^a}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta b_i^a}=0$$

• $i$时刻陀螺仪偏置残差$r_{b_g}$对陀螺仪偏置的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta b_i^g}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta b_i^g}=-I$$

2.2.2 残差对j时刻各状态量的雅可比

（1）位移残差在$j$时刻对各状态量的雅可比

• $j$时刻位移残差$r_p$对位移的雅可比

$$\frac{\partial r_p}{\partial \delta p_j{p}_j^{,}}=\frac{\partial [q_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}]}{\partial \delta p_j{p}_j^{,}}=\frac{\partial q_{w{b}_i}^{-1}(p_{w{b}_j})}{\partial \delta p_j{p}_j^{,}}=q_{w{b}_i}^{-1}$$

• $j$时刻位移残差$r_p$对姿态的雅可比

$$\frac{\partial r_p}{\partial \delta {\theta }_{b_j{b}_j^{,}}}=\frac{\partial [q_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}]}{\partial \delta {\theta }_{b_j{b}_j^{,}}}=0$$

• $j$时刻位移残差$r_p$对速度的雅可比

$$\frac{\partial r_p}{\partial \delta v_j^w}=\frac{\partial [q_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}]}{\partial \delta v_j^w}=0$$

• $j$时刻位移残差$r_p$对加速度计偏置的雅可比

$$\frac{\partial r_p}{\partial \delta b_i^a}=\frac{\partial [q_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}]}{\partial \delta b_i^a}=0$$

• $j$时刻位移残差$r_p$对陀螺仪偏置的雅可比

$$\frac{\partial r_p}{\partial \delta b_i^g}=\frac{\partial [q_{w{b}_i}^{-1}(p_{w{b}_j}-p_{w{b}_i}-v_i^w\Delta t+\frac{1}{2}{g}^w\Delta t^2)-{\alpha }_{b_i{b}_j}]}{\partial \delta b_i^g}=0$$

（2）姿态残差在$j$时刻对各状态量的雅可比

• $j$时刻姿态残差$r_q$对位移的雅可比

$$\frac{\partial r_q}{\partial \delta p_j{p}_j^{,}}=\frac{\partial 2[q_{b_i{b}_j}^{-1}\otimes q_{w{b}_i}^{-1}\otimes q_{w{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_j{b}_j^{,}}\end{array}\right]{]}_{xyz}}{\partial \delta p_j{p}_j^{,}}=0$$

• $j$时刻姿态残差$r_q$对姿态的雅可比

$$\begin{gathered} \frac{\partial r_q}{\partial \delta {\theta }_{b_j{b}_j^{,}}}=\frac{\partial 2[q_{b_i{b}_j}^{-1}\otimes q_{w{b}_i}^{-1}\otimes q_{w{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_j{b}_j^{,}}\end{array}\right]{]}_{xyz}}{\partial \delta {\theta }_{b_i{b}_i^{,}}}=\frac{\partial 2[[q_{b_i{b}_j}^{-1}\otimes q_{w{b}_i}^{-1}\otimes q_{w{b}_j}{]}_L\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_j{b}_j^{,}}\end{array}\right]{]}_{xyz}}{\partial \delta {\theta }_{b_i{b}_i^{,}}} \\ =2\left[\begin{array}{cc}0& I\end{array}\right][q_{b_i{b}_j}^{-1}\otimes q_{w{b}_i}^{-1}\otimes q_{w{b}_j}{]}_L\left[\begin{array}{c}0\\ \frac{1}{2}I\end{array}\right] \end{gathered}$$

• $j$时刻姿态残差$r_q$对速度的雅可比

$$\frac{\partial r_q}{\partial \delta v_j^w}=\frac{\partial 2[q_{b_i{b}_j}^{-1}\otimes q_{w{b}_i}^{-1}\otimes q_{w{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_j{b}_j^{,}}\end{array}\right]{]}_{xyz}}{\partial \delta v_j^w}=0$$

• $j$时刻姿态残差$r_q$对加速度计偏置的雅可比

$$\frac{\partial r_q}{\partial \delta b_j^a}=\frac{\partial 2[q_{b_i{b}_j}^{-1}\otimes q_{w{b}_i}^{-1}\otimes q_{w{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_j{b}_j^{,}}\end{array}\right]{]}_{xyz}}{\partial \delta b_j^a}=0$$

• $j$时刻姿态残差$r_q$对陀螺仪偏置的雅可比

$$\frac{\partial r_q}{\partial \delta b_j^g}=\frac{\partial 2[q_{b_i{b}_j}^{-1}\otimes q_{w{b}_i}^{-1}\otimes q_{w{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_j{b}_j^{,}}\end{array}\right]{]}_{xyz}}{\partial \delta b_j^g}=0$$

（3）速度残差在$j$时刻对各状态量的雅可比

• $j$时刻速度残差$r_v$$对位移的雅可比

$$\frac{\partial r_v}{\partial \delta p_j{p}_j^{,}}=\frac{\partial q_{w{b}_i}^{-1}(v_j^w-v_i^w+g^w\Delta t)-{\beta }_{b_i{b}_j}}{\partial \delta p_j{p}_j^{,}}=0$$

• $j$时刻速度残差$r_v$对姿态的雅可比

$$\frac{\partial r_v}{\partial \delta {\theta }_{b_j{b}_j^{,}}}=\frac{\partial q_{w{b}_i}^{-1}(v_j^w-v_i^w+g^w\Delta t)-{\beta }_{b_i{b}_j}}{\partial \delta {\theta }_{b_j{b}_j^{,}}}=0$$

• $j$时刻速度残差$r_v$对速度的雅可比

$$\frac{\partial r_v}{\partial \delta v_j^w}=\frac{\partial q_{w{b}_i}^{-1}(v_j^w-v_i^w+g^w\Delta t)-{\beta }_{b_i{b}_j}}{\partial \delta v_j^w}=q_{w{b}_i}^{-1}$$

• $j$时刻速度残差$r_v$对加速度计偏置的雅可比

$$\frac{\partial r_v}{\partial \delta b_j^a}=\frac{\partial q_{w{b}_i}^{-1}(v_j^w-v_i^w+g^w\Delta t)-{\beta }_{b_i{b}_j}}{\partial \delta b_j^a}=0$$

• $j$时刻速度残差$r_v$对陀螺仪偏置的雅可比

$$\frac{\partial r_v}{\partial \delta b_j^g}=\frac{\partial q_{w{b}_i}^{-1}(v_j^w-v_i^w+g^w\Delta t)-{\beta }_{b_i{b}_j}}{\partial \delta b_j^g}=0$$

（4）加速度计偏置残差在$j$时刻对各状态量的雅可比

• $j$时刻加速度计偏置残差$r_{b_a}$对位移的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta p_j{p}_j^{,}}=\frac{\partial (b_j^a-b_i^a)}{\partial \delta p_j{p}_j^{,}}=0$$

• $j$时刻加速度计偏置残差$r_{b_a}$对姿态的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta {\theta }_{b_j{b}_j^{,}}}=\frac{\partial (b_j^a-b_i^a)}{\partial \delta {\theta }_{b_j{b}_j^{,}}}=0$$

• $j$时刻加速度计偏置残差$r_{b_a}$对速度的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta v_j^w}=\frac{\partial (b_j^a-b_i^a)}{\partial \delta v_j^w}=0$$

• $j$时刻加速度计偏置残差$r_{b_a}$对加速度计偏置的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta b_j^a}=\frac{\partial (b_j^a-b_i^a)}{\partial \delta b_j^a}=I$$

• $j$时刻加速度计偏置残差$r_{b_a}$对陀螺仪偏置的雅可比

$$\frac{\partial r_{b_a}}{\partial \delta b_j^g}=\frac{\partial (b_j^a-b_i^a)}{\partial \delta b_j^g}=0$$

（5）陀螺仪偏置残差在$j$时刻对各状态量的雅可比

• $j$时刻陀螺仪偏置残差$r_{b_g}$对位移的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta p_j{p}_j^{,}}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta p_j{p}_j^{,}}=0$$

• $j$时刻陀螺仪偏置残差$r_{b_g}$对姿态的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta {\theta }_{b_j{b}_j^{,}}}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta {\theta }_{b_j{b}_j^{,}}}=0$$

• $j$时刻陀螺仪偏置残差$r_{b_g}$对速度的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta v_j^w}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta v_j^w}=0$$

• $j$时刻陀螺仪偏置残差$r_{b_g}$对加速度计偏置的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta b_j^a}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta b_j^a}=0$$

• $j$时刻陀螺仪偏置残差$r_{b_g}$对陀螺仪偏置的雅可比

$$\frac{\partial r_{b_g}}{\partial \delta b_j^g}=\frac{\partial (b_j^g-b_i^g)}{\partial \delta b_j^g}=I$$

2.2.3 残差对各状态量的雅可比计算

（1）代码分析

​ 前面已推导出残差对$i$时刻和$j$​时刻各状态量的雅可比形式，将总雅可比矩阵进行分块，记残差对位姿的雅可比为$\frac{\partial e}{\partial P}$，残差对速度、加速度计零偏以及陀螺仪零偏的雅可比为$\frac{\partial e}{\partial V}$。由于残差维度为15，位置和姿态的自由度分别为3和4，故$\frac{\partial e}{\partial P}$的维度是$15\times 7$。同理，由于速度、加速度计零偏以及陀螺仪零偏均为3，故$\frac{\partial e}{\partial V}$的维度是$15\times 9$。雅可比矩阵由残差对$k$时刻和$k+1$时刻的雅可比组成，形式如下：
$$\left[\begin{array}{cccc}\frac{\partial e}{\partial P_k}& \frac{\partial e}{\partial V_k}& \frac{\partial e}{\partial P_{k+1}}& \frac{\partial e}{\partial V_{k+1}}\end{array}\right]$$
（2）代码实现

// 15: 维护两帧之间的PVQ + 加速度零偏 + 角速度零偏
// 7: 位姿自由度
// 9: 速度与零偏自由度
class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
{
    public:
    IMUFactor() = delete;
    IMUFactor(IntegrationBase* _pre_integration):pre_integration(_pre_integration)
    {
    }
    /**
     * @brief  使用ceres解析求导，必须重载这个函数
     * 
     * @param[in] parameters 这是一个二维数组，每个参数块都是一个double数组，而一个观测会对多个参数块形成约束
     * @param[in] residuals 残差的计算结果，是一个一维数组，残差就是该观测量和约束的状态量通过某种关系形成残差
     * @param[in] jacobians 残差对参数块的雅克比矩阵，这是一个二维数组，其中对任意一个参数块的雅克比矩阵都是一个一维数组
     * @return true 
     * @return false 
     */
    virtual bool Evaluate(double const *const *parameters, 
                          double *residuals, 
                          double **jacobians) const
    {
        // 便于后续计算，把double类型的参数块都转换成eigen
        // imu预积分约束的参数是相邻两帧的位姿 速度和零偏  i表示前一帧, j表示后一帧
        Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], 
                              parameters[0][4], parameters[0][5]);

        Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
        Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
        Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

        Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
        Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], 
                              parameters[2][4], parameters[2][5]);

        Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
        Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
        Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);

        // 定义残差大小  将double类型的残差映射成15*1的eigen对象
        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);  
        // 得到残差(具体实现在下面)
        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);
// 因为ceres没有g2o设置信息矩阵的接口，因此置信度直接乘在残差上，这里通过LLT分解，相当于将信息矩阵开根号
// 也就是说进行如下转换: 令P = pp^T, 则e^TPe = e^Tpp^Te = (p^Te)^T(p^Te)  p^Te作为新的残差
        // 其中, P为预积分的协方差矩阵的逆(即信息矩阵)
        Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
        // 这就是带有信息矩阵的残差
        residual = sqrt_info * residual;
        
        // 下面进行雅可比的计算
        if (jacobians)
        {
            double sum_dt = pre_integration->sum_dt;
            // 预先求出对偏置的雅可比
            Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
            Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);

            Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);

            Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);
            Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);

            if (pre_integration->jacobian.maxCoeff() > 1e8 || 
                pre_integration->jacobian.minCoeff() < -1e8)
            {
                ROS_WARN("numerical unstable in preintegration");
            }
            
            if (jacobians[0])
            {
                // 第0列 —— 第6列(前7列)计算残差对第i时刻位姿的雅可比(平移3个量, 旋转4个量)
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> 
                    									jacobian_pose_i(jacobians[0]);
                jacobian_pose_i.setZero();
                // i时刻平移残差对平移量的雅可比  Qi: q_{b_i}^w
                jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
                // i时刻平移残差对姿态(旋转矩阵R)的雅可比  
                // R_w^{b_i} * [0.5gdelta_t^2 + Pj - Pi - vi * delta_t]
                // Utility::skewSymmetric的作用是将向量转换成反对称矩阵
                jacobian_pose_i.block<3, 3>(O_P, O_R)=Utility::skewSymmetric(Qi.inverse()
								  * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));

#if 0
                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).
                    												  toRotationMatrix(); 
#else           // 零偏修正后的旋转预积分量: q_{b_j}^{b_i}
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                // Qleft(Qj.inverse() * Qi): [q_w^{b_j} * q_{b_I}^w]_L 左乘矩阵
                // Qright(corrected_delta_q): 旋转预积分量corrected_delta_q的右乘矩阵
                // bottomRightCorner<3, 3>: 取右下角的3*3矩阵为虚部
                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif
                // i时刻速度残差对姿态(旋转矩阵R)的雅可比  Qi: q_{b_i}^w
                jacobian_pose_i.block<3, 3>(O_V, O_R) = 
                           Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
				// 求出的雅可比矩阵再乘上前面计算出来的信息矩阵作为权重
                jacobian_pose_i = sqrt_info * jacobian_pose_i;  

                if (jacobian_pose_i.maxCoeff()>1e8 || jacobian_pose_i.minCoeff() < -1e8)
                {
                    ROS_WARN("numerical unstable in preintegration");
                }
            }
            if (jacobians[1])
            {
                // 第7列 —— 第15列(共9列)计算残差对第i时刻速度v_i、加速度偏置b_{a_i}、角速度偏置b_{w_i}的雅可比(3个3维，共9维)
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> 
                    						          jacobian_speedbias_i(jacobians[1]);
                jacobian_speedbias_i.setZero();
                // j时刻平移残差对速度量的雅可比: -q_w^{b_i} * \delta_t  Qi: q_{b_i}^w
                jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -
                    						    Qi.inverse().toRotationMatrix() * sum_dt;
                // j时刻平移残差对加速度偏置的雅可比: -J_{b_a}^{\alpha}
                jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
                // j时刻平移残差对角速度偏置的雅可比: -J_{b_g}^{\alpha}
                jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;
#if 0
            jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else  
            // [(q_{b_j}^w)^{-1} \otimes q_{b_i}^w \otimes q_{b_i}^{b_i}]_L * J_{b_g}^q
            // 其中, q_{b_j}^w由Qj表示    q_{b_i}^w由Qi表示    
            // 预积分量q_{b_i}^{b_i}由pre_integration->delta_q表示
            // dq_dbg表示姿态对角速度偏置的雅可比J_{b_g}^q
            // bottomRightCorner<3, 3>()取右下角的3*3矩阵，即取虚部
                jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft
       (Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif
                // i时刻速度残差对速度量的雅可比: -q_w^{b_i}
                jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -
                    								     Qi.inverse().toRotationMatrix();
                // i时刻速度残差对加速度偏置的雅可比: -J_{b_a}^{\beta}
                jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
                // i时刻速度残差对角速度偏置的雅可比: -J_{b_g}^{\beta}
                jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;

                // i时刻加速度偏置残差对加速度偏置的雅可比: -I
                jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -
                    									    Eigen::Matrix3d::Identity();
                // i时刻角速度偏置残差对角速度偏置的雅可比: -I
                jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -
                    										Eigen::Matrix3d::Identity();
				// 求出的雅可比矩阵再乘上前面计算出来的信息矩阵作为权重
                jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;  
            }
            if (jacobians[2])
            {
                // 第16列 —— 第22列(共7列)计算残差对第j时刻位姿的雅可比(平移3个量, 旋转4个量)
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> 
                    									jacobian_pose_j(jacobians[2]);
                jacobian_pose_j.setZero();
                // j时刻平移残差对平移量的雅可比  Qj: q_{b_j}^w
                jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();

#if 0
            jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
                // 零偏修正后的旋转预积分量: q_{b_j}^{b_i}
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                // Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj): [q_{b_i}^{b_j} \otimes q_w^{b_i} \otimes q_{b_j}^w]_L 左乘矩阵
                // bottomRightCorner<3, 3>: 取右下角的3*3矩阵为虚部
                jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft
             (corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif
                jacobian_pose_j = sqrt_info * jacobian_pose_j;
            }
            if (jacobians[3])
            {
                // 第23列 —— 第31列(共9列)计算残差对第j时刻速度v_j、加速度偏置b_{a_j}、角速度偏置b_{w_j}的雅可比(3个3维，共9维)
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> 
                    								jacobian_speedbias_j(jacobians[3]);
                jacobian_speedbias_j.setZero();
                // j时刻速度残差对速度量的雅可比  q_w^{b_i}
                jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = 
                    								Qi.inverse().toRotationMatrix();
                // j时刻加速度偏置残差对加速度偏置的雅可比
                jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = 
                    								Eigen::Matrix3d::Identity();
                // j时刻角速度偏置残差对角速度偏置的雅可比
                jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = 
                    								Eigen::Matrix3d::Identity();
                jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;
            }
        }
        return true;
    }
};