前言：

對第k幀和第k+1幀之間所有的IMU進行積分，可得到第K+1幀的PVQ（位置、速度、旋轉），作爲視覺估計的初始值。

每次qwbt優化更新後，都要重新進行積分，運算量較大。將積分模型轉爲預積分模型：

PVQ積分公式中的積分項變爲相對於第i時刻的姿態，而不是相對於世界座標系的姿態。

一、IMU離散中值預積分理論基礎

1、沒預積分：PVQ連續、離散積分

1、IMU模型

2、連續時間IMU運動模型，積分 PVQ（兩幀之間）

3、PVQ的中值離散積分（前後IMU）

2、預積分：PVQ連續、離散積分、預積分誤差、bias 預積分量

5、bias 預積分量（bias發生變化）

1、預積分的Jacobian（rv、rq）

2、預積分的協方差

三、processIMU函數

IMU 預積分IntegrationBase類

5、中值積分midPointIntegration（）

6、repropagate（）新的bias重新計算預積分

7、imu_factor.h

一、IMU離散中值預積分理論基礎

1、沒預積分：PVQ連續、離散積分

1、IMU模型

測量值：加速度計 $\widetilde{a^{b}}$ 、陀螺儀 $\widetilde{w^{b}}$ , 加上了bias遊走 $b^{a}b^{g}$ 和隨機白噪聲 $n^{a}n^{g}$ 。

真實值：加速度計a、陀螺儀w。

實際情況下，可以獲得測量值a^和w^，需要反推真實值。一般忽略隨機遊走高斯噪聲n

w= $\widetilde{w^{b}}$ -bg; a=qwb( $\widetilde{a^{b}}$ -ba)-gw;

2、連續時間IMU運動模型，積分 PVQ（兩幀之間）

將第k幀和第k+1幀所有的IMU進行積分，可得到第k+1幀的 PVQ，作爲視覺估計的初始值。

$a^{bt}$ 和 $w^{bt}$ 是IMU測量的加速度和角速度，相對於Body座標系。

world座標系是IMU所在的慣導系。Body系和慣導系關係爲：

3、PVQ的中值離散積分（前後IMU）

從第 i個IMU時刻到第 i+1個IMU時刻的積分過程。兩個相鄰時刻k到k+1的位姿是由第k時刻測量值a^,w^計算得出的。

這與Estimator::processIMU（）函數中Ps[j]、Rs[j]、Vs[j]是一致的，代碼中j就是此處的i+1

IMU積分出來第 j 時刻數值作爲第 j 幀圖像初始值。

歐拉法

中值法

2、預積分：PVQ連續、離散積分、預積分誤差、bias 預積分量

1、 PVQ連續積分（預積分）

每次qwbt優化更新後，都要重新進行積分，運算量較大。

將積分模型轉爲預積分模型：

PVQ積分公式中的積分項變爲相對於第i時刻的姿態，而不是相對於世界座標系的姿態

2、預積分量

IMU預積分量只與IMU測量值有關。（加速度計 $\widetilde{a^{b}}$ 、陀螺儀 $\widetilde{w^{b}}$ ）

3、預積分中值離散形式

中值法：k到k+1時刻位姿由兩時刻的測量值a w的平均值來計算。

4、預積分誤差

一段時間內IMU構建的預積分量作爲測量值，與估計值進行相減。

5、bias 預積分量（bias發生變化）

因爲 i 時刻的 bias 相關的預積分計算是通過迭代一步一步累計遞推的，可以算但是太複雜。所以對於預積分量直接在 i 時刻的 bias 附近用一階泰勒展開來近似，而不用真的去迭代計算。

二、預積分的Jacobian和協方差

1、預積分的誤差以及線性傳遞方程

預積分模型減小了計算量，但是丟失了一些東西。比如當我們用1個結果代替100個數據點時，轉化之前100個點每一個點的不確定度是知道的（IMU數據作爲測量值的噪聲方差能夠標定），但是轉化後的1個結果不確定度不知。100個數據積分形成的預積分量方差是多少？這就需要我們在得到IMU預積分後，推導預積分量的協方差，需要直到IMU噪聲和預積分量之間的線性遞推關係。

IMU在每一個時刻積分出來的值是有誤差的，下面對誤差進行分析。

1.預積分誤差

一段時間內IMU構建的預積分量作爲測量值，與估計值進行相減。

上面誤差中位移，速度，偏置都是直接相減得到。第二項是關於四元數的旋轉誤差，其中 [·] xyz 表示只取四元數的虛部 (x,y,z) 組成的三維向量。

2.相鄰時刻誤差的線性傳遞方程

誤差的傳遞分爲兩部分：1）當前時刻誤差傳遞給下一時刻；2）當前時刻測量噪聲傳遞給下一時刻。

狀態量誤差爲：

測量噪聲爲：

3.預積分協方差矩陣

協方差矩陣矩陣通過遞推爲

其中， $\sum n$ 是測量噪聲的協方差矩陣，方差從i=0時刻開始遞推。

F和G是兩時刻間的協方差傳遞矩陣， $\delta (.)$ 表示各時刻的誤差。

2、預積分的Jacobian和協方差

兩幀之間PVQ和bias變化量的殘差爲：

殘差是15維的，狀態量是7+9+7+9維的，所以說，每增加一個IMU約束，總的Jacobian矩陣增加了15行，增加了7+9+7+9列！(需要注意的是，四元數是4維的，但是Localdemension是3維的)。

1、預積分的Jacobian（rv、rq）

優化變量主要包括第i、j時刻的p、q、v、ba、bg：

殘差對狀態量的Jacobian是什麼，對應位置補充上這個J的矩陣塊就行了，其他位置還是0。

VINS源碼中，在imu_factor.h中的class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> 的函數virtual bool Evaluate()中實現，其中parameters[0~3]分別對應了以上4組優化變量的參數塊。對於 Evaluate 輸入 double const *const *parameters, parameters[0], parameters[1], parameters[2],parameters[3]分別對應 4 個輸入參數, 它們的長度依次是 7,9,7,9，

方式1：整個殘差對某個變量求導；

例如： $\frac{\partial r_{B}}{\partial p_{b}^{w}}$ 指的是 $\begin{bmatrix} \frac{\partial r_{p}}{\partial p_{b}^{w}}\\\frac{\partial r_{v}}{\partial p_{b}^{w}} \\\frac{\partial r_{q}}{\partial p_{b}^{w}} \\ \frac{\partial r_{ba}}{\partial p_{b}^{w}}\\ \frac{\partial r_{bg}}{\partial p_{b}^{w}}\end{bmatrix}$

主要的四部分（參考崔華坤筆記）爲：

方式2：推導關於第i、j幀PVQ的Jacobian（以rv和rq爲例）

方式爲： $\frac{\partial r_{v}}{\partial \delta(p ) }$ 、 $\frac{\partial r_{v}}{\partial \delta(v) }$ 、 $\frac{\partial r_{v}}{\partial \delta(\theta ) }$ 、 $\frac{\partial r_{v}}{\partial \delta(b_{a}) }$ 、 $\frac{\partial r_{v}}{\partial \delta(b_{g}) }$ 殘差某一行對5個變量分別求導。

下面爲詳細推導，如果不感興趣，可以直接記住上面方式1的四部分即可。

由於 rp 和 rv 的誤差形式很相近，對各狀態量求導的 Jacobian 形式也很相似，所以這裏只對 rv 的推導進行詳細介紹。

2、預積分的協方差

三、processIMU函數

IMU預積分，中值積分得到當前PQV作爲優化初值

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
    // 1.imu未進來數據
    if (!first_imu)
    {
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }
    // 2.IMU 預積分類對象還沒出現，創建一個
    if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
    
    if (frame_count != 0)
    {   // 3.預積分操作
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)
            tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

        // 4.dt、加速度、角速度加到buf中
        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        int j = frame_count; 
        // 5.採用的是中值積分的傳播方式        
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;// a0=Q(a^-ba)-g 已知上一幀imu速度
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];// w=0.5(w0+w1)-bg
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;// a1 當前幀imu速度
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);// 中值積分下的加速度a=1/2(a0+a1)
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;// P=P+v*t+1/2*a*t^2
        Vs[j] += dt * un_acc;// V=V+a*t
    }
    // 6.更新上一幀的加速度和角速度
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

IMU 預積分IntegrationBase類

1、構造函數

預積分類：加速度計、陀螺儀、線性加速度計ba、陀螺儀bg、雅克比矩陣初始化、協方差矩陣15*15、dt、PVQ

// 預積分類：加速度計、陀螺儀、線性加速度計ba、陀螺儀bg、雅克比矩陣初始化、協方差矩陣15*15、dt、PVQ
    IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                    const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
        : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
          linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
            jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
          sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}

    {
        noise = Eigen::Matrix<double, 18, 18>::Zero();
        noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
    }

2、push_back()函數

void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
    {
        dt_buf.push_back(dt);
        acc_buf.push_back(acc);
        gyr_buf.push_back(gyr);
        propagate(dt, acc, gyr);
    }

3、evaluate（）函數

構建殘差

// 構建殘差residuals
    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;// 殘差

        Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
        Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);

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

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

        Eigen::Vector3d dba = Bai - linearized_ba;
        Eigen::Vector3d dbg = Bgi - linearized_bg;

        Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
        Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
        Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

        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();
        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;
    }

4、propagate（）函數

IMU預積分傳播方程

積分計算兩個關鍵幀之間IMU測量的變化量：

旋轉delta_q 速度delta_v 位移delta_p

加速度的biaslinearized_ba 陀螺儀的Bias linearized_bg

同時維護更新預積分的Jacobian和Covariance,計算優化時必要的參數

預積分傳播方程，在預積分傳播方程propagate中使用中點積分方法midPointIntegration計算預積分的測量值，中點積分法中主要包含兩個部分，分別是得到狀態變化量result_delta_q，result_delta_p，result_delta_v，result_linearized_ba，result_linearized_bg和得到跟新協方差矩陣和Jacobian矩陣（注意，雖然得到了雅各比矩陣和協方差矩陣，但是還沒有求殘差和修正偏置一階項的狀態變量），由於使用的是中點積分，所以需要上一個時刻的IMU數據，包括測量值加速度和角速度以及狀態變化量，初始值由構造函數提供。需要注意的是這裏定義的delta_p等是累積的變化量，也就是說是從i時刻到當前時刻的變化量，這個纔是最終要求的結果（爲修正偏置一階項），而result_delta_q等只是一個暫時的變量，最後殘差和雅可比矩陣、協方差矩陣保存在pre_integrations中，還有一個函數這裏暫時還沒有用到，是在優化的時候才被調用的，但是其屬於預積分的內容，evaluate函數在這個函數裏面進行了狀態變化量的偏置一階修正以及殘差的計算。

步驟2預積分公式（3）未考慮誤差，提供imu計算的當前旋轉，位置，速度，作爲優化的初值

求狀態向量對bias的Jacobian，當bias變化較小時，使用Jacobian去更新狀態；否則需要以當前imu爲參考系，重新預積分，對應repropagation()。同時，需要計算error state model中誤差傳播方程的係數矩陣F和V：

   void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
    {
        dt = _dt;
        acc_1 = _acc_1;
        gyr_1 = _gyr_1;
        Vector3d result_delta_p;
        Quaterniond result_delta_q;
        Vector3d result_delta_v;
        Vector3d result_linearized_ba;
        Vector3d result_linearized_bg;

        midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
                            linearized_ba, linearized_bg,
                            result_delta_p, result_delta_q, result_delta_v,
                            result_linearized_ba, result_linearized_bg, 1);

        //checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
        //                    linearized_ba, linearized_bg);
        delta_p = result_delta_p;
        delta_q = result_delta_q;
        delta_v = result_delta_v;
        linearized_ba = result_linearized_ba;
        linearized_bg = result_linearized_bg;
        delta_q.normalize();
        sum_dt += dt;
        acc_0 = acc_1;
        gyr_0 = gyr_1;  
     
    }

最主要的還是在midPointIntegration（）函數中。

5、中值積分midPointIntegration（）

IMU預積分中採用中值積分遞推Jacobian和Covariance

構造誤差的線性化遞推方程，得到Jacobian和Covariance遞推公式-> Paper 式9、10、11

void midPointIntegration(double _dt, 
                            const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                            const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
                            const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
                            const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
                            Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
                            Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
    {
        //ROS_INFO("midpoint integration");
        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        result_delta_v = delta_v + un_acc * _dt;
        result_linearized_ba = linearized_ba;
        result_linearized_bg = linearized_bg;         

        if(update_jacobian)
        {
            Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
            Vector3d a_0_x = _acc_0 - linearized_ba;
            Vector3d a_1_x = _acc_1 - linearized_ba;
            Matrix3d R_w_x, R_a_0_x, R_a_1_x;

            //反對稱矩陣
            R_w_x<<0, -w_x(2), w_x(1),
                w_x(2), 0, -w_x(0),
                -w_x(1), w_x(0), 0;
            R_a_0_x<<0, -a_0_x(2), a_0_x(1),
                a_0_x(2), 0, -a_0_x(0),
                -a_0_x(1), a_0_x(0), 0;
            R_a_1_x<<0, -a_1_x(2), a_1_x(1),
                a_1_x(2), 0, -a_1_x(0),
                -a_1_x(1), a_1_x(0), 0;
            // 對F賦值
            MatrixXd F = MatrixXd::Zero(15, 15);
            F.block<3, 3>(0, 0) = Matrix3d::Identity();
            F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                                  -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
            F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
            F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
            F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
            F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
            F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
            F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                                  -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
            F.block<3, 3>(6, 6) = Matrix3d::Identity();
            F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
            F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
            F.block<3, 3>(9, 9) = Matrix3d::Identity();
            F.block<3, 3>(12, 12) = Matrix3d::Identity();
            //cout<<"A"<<endl<<A<<endl;
            // 對V賦值
            MatrixXd V = MatrixXd::Zero(15,18);
            V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;
            V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;
            V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
            V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);
            V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;
            V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;
            V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;
            V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);
            V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
            V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;

            //step_jacobian = F;
            //step_V = V;
            // Jacobian和協方差矩陣
            jacobian = F * jacobian;
            covariance = F * covariance * F.transpose() + V * noise * V.transpose();
        }

    }

這部分對F和V的賦值就是誤差的線性傳遞函數

之後求解Jacobian和協方差分別爲：

6、repropagate（）新的bias重新計算預積分

void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
    {
        sum_dt = 0.0;
        acc_0 = linearized_acc;// 舊的加速度和陀螺儀
        gyr_0 = linearized_gyr;
        delta_p.setZero();
        delta_q.setIdentity();
        delta_v.setZero();
        linearized_ba = _linearized_ba;// 更新Bias
        linearized_bg = _linearized_bg;
        jacobian.setIdentity();
        covariance.setZero();
        for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)
            propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
    }

7、imu_factor.h

主函數爲

 virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const

1、填充優化變量，i和j時刻的4組優化變量參數塊

// 1、優化變量：i和j時刻的4組優化變量參數塊
        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]);

2、構建IMU殘差residual

主要通過調用 pre_integration->evaluate，預積分模塊中evaluate函數。也就是上面的3 evaluate（）函數

        // 2、構建IMU殘差residual
        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);// 已知殘差
        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);

        // LLT分解，residual 還需乘以信息矩陣的sqrt_info
        // 因爲優化函數其實是d=r^T P^-1 r ，P表示協方差，而ceres只接受最小二乘優化
        // 因此需要把P^-1做LLT分解，使d=(L^T r)^T (L^T r) = r'^T r
        Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
        residual = sqrt_info * residual;

3、獲取預積分的誤差遞推函數中pqv關於ba、bg的Jacobian

// 3、獲取預積分的誤差遞推函數中pqv關於ba、bg的Jacobian
            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");
                //std::cout << pre_integration->jacobian << std::endl;
///                ROS_BREAK();
            }

接下來便是對4個模塊進行求解

4、第i幀的IMU位姿 pbi、qbi

 // 4、第i幀的IMU位姿 pbi、qbi
            if (jacobians[0])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
                jacobian_pose_i.setZero();

                jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
                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
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif

                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");
                    //std::cout << sqrt_info << std::endl;
                    //ROS_BREAK();
                }
            }

5、第i幀的imu速度vbi、bai、bgi

 // 6、第i幀的imu速度vbi、bai、bgi
            if (jacobians[1])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
                jacobian_speedbias_i.setZero();

                jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
                jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
                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
                //Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                //jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;
                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

                jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
                jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
                jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;

                jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;

                //ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
            }

6、第j幀的IMU位姿 pbj、qbj

 // 7、第j幀的IMU位姿 pbj、qbj
            if (jacobians[2])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
                jacobian_pose_j.setZero();

                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
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                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;

                //ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);
            }

7、第j幀的IMU速度vbj、baj、bgj

// 8、第j幀的IMU速度vbj、baj、bgj
            if (jacobians[3])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
                jacobian_speedbias_j.setZero();

                jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();

                jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;

                //ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);
            }

VINS-Mono 代碼詳細解讀——IMU預積分的殘差、Jacobian和協方差 processIMU（）+intergrationBase類+imu_factor.h

一、IMU離散中值預積分理論基礎

1、沒預積分：PVQ連續、離散積分

1、IMU模型

2、連續時間IMU運動模型，積分 PVQ（兩幀之間）

3、PVQ的中值離散積分（前後IMU）

2、預積分：PVQ連續、離散積分、預積分誤差、bias 預積分量

1、 PVQ連續積分（預積分）

2、預積分量

3、預積分中值離散形式

4、預積分誤差

5、bias 預積分量（bias發生變化）

二、預積分的Jacobian和協方差

1、預積分的誤差以及線性傳遞方程

1.預積分誤差

2.相鄰時刻誤差的線性傳遞方程

3.預積分協方差矩陣

2、預積分的Jacobian和協方差

1、預積分的Jacobian（rv、rq）

2、預積分的協方差

三、processIMU函數

IMU 預積分IntegrationBase類

1、構造函數

2、push_back()函數

3、evaluate（）函數

4、propagate（）函數

5、中值積分midPointIntegration（）

6、repropagate（）新的bias重新計算預積分

7、imu_factor.h

一鍵自動化博客發佈工具,用過的人都說好(頭條篇)

VINS-Mono 代碼詳細解讀——初始化1：視覺SFM詳解 processImage（）+initialStructure（）

多傳感器融合定位2（封裝KF和EKF）

Endnote連接Word自動插入畢業論文參考文獻

《wiki官網教程》2 編寫簡單的服務器service和客戶端 client(C++)

VINS-Mono 代碼詳細解讀——基於滑動窗口的緊耦合後端非線性優化 optimization()+滑動窗slideWindow()

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結

VINS-Mono 代碼詳細解讀——IMU預積分的殘差、Jacobian和協方差 processIMU（）+intergrationBase類+imu_factor.h

一、IMU離散中值預積分理論基礎

1、沒預積分：PVQ連續、離散積分

1、IMU模型

2、連續時間IMU運動模型，積分 PVQ（兩幀之間）

3、PVQ的中值離散積分（前後IMU）

2、預積分：PVQ連續、離散積分、預積分誤差、bias 預積分量

1、 PVQ連續積分（預積分）

2、預積分量

3、 預積分中值離散形式

4、預積分誤差

5、bias 預積分量（bias發生變化）

二、預積分的Jacobian和協方差

1、預積分的誤差以及線性傳遞方程

1.預積分誤差

2.相鄰時刻誤差的線性傳遞方程

3.預積分協方差矩陣

2、預積分的Jacobian和協方差

1、預積分的Jacobian（rv、rq）

2、預積分的協方差

三、processIMU函數

IMU 預積分IntegrationBase類

1、構造函數

2、push_back()函數

3、evaluate（）函數

4、propagate（）函數

5、中值積分midPointIntegration（）

6、repropagate（）新的bias重新計算預積分

7、imu_factor.h

3、預積分中值離散形式