4.IMU預積分
IMU預積分主要乾了2件事，第一個是IMU預積分獲得α、β、γ值，另一個是誤差傳遞函數的獲取。本部分的流程圖如下圖所示。
各個部分的講解如下鏈接：
【SLAM】VINS-MONO解析——feature_tracker
【SLAM】VINS-MONO解析——vins_estimator
【SLAM】VINS-MONO解析——sliding window
【SLAM】VINS-MONO解析——迴環檢測
【SLAM】VINS-MONO解析——對vins-mono的修改使流程邏輯更清晰
【SLAM】VINS-MONO解析——基於vins-mono的slam系統開發
4.1 IMU預積分
這部分內容主要出現在vins_estimator/src/factor/integration_base.h中。
4.1.1 連續形式
第一個問題，爲什麼要IMU積分？
IMU獲取的是加速度和角速度，通過對IMU測量量的積分操作，能夠獲得機器人的位姿信息。
IMU測量值包括加速度計得到的線加速度和陀螺儀得到的角速度。
其中 t下標表示在IMU座標系下，並受到加速度偏置ba、陀螺儀偏置bw和附加噪聲na、nw的影響。線加速度是重力加速度和物體加速度的合矢量，上標^代表是IMU的測量量，沒有上標的值代表的是真實的量。
附加噪聲是滿足高斯噪聲的。
加速度計偏置和陀螺儀偏置被定義爲爲隨機遊走並隨着時間變化的，它的導數滿足高斯分佈。
對於圖像幀k和k+1，體座標系對應爲bk和bk+1，位置、速度和方向狀態值PVQ可以根據[tk,tk+1]時間間隔內的IMU測量值，在世界座標系下進行傳遞的。
等號右邊的三個量是IMU積分需要求的量，分別是bk+1時刻下，IMU座標原點在世界座標系上的座標，速度和旋轉角度，它們都是由bk時刻對應的值加上從bk到bk+1的變化量求得的，而這個變化量是由IMU積分獲得的。
第二個問題，爲什麼要IMU“預”積分？
IMU的採樣頻率是高於圖像幀的發佈頻率的，所以相鄰兩個圖像幀之間的IMU信息需要積分從而與視覺信息對齊。
Rw<-t,也就是qw<-t，是需要被優化的量，而這個優化的量，是在IMU積分符號內部的，在後續的優化過程中，這個值是在不斷的變化，所以需要一遍遍的重新IMU積分才能獲得真正精確的PVQ值。爲了減少IMU積分次數，就希望這個被優化的量不要出現在積分符號裏，所以就採用IMU預積分的方式。把上面三個等式，全部等號兩邊乘以Rbk<-w，這樣積分裏面的內容只跟IMU的測量量有關，而與被優化的狀態量無關，這就是IMU預積分。
上式中，等號左邊是要求的值，等號右邊括號裏的值是不需要積分的，也是能直接算出來的，α、β、γ是需要積分的量，也就是IMU預積分主要操作的值。
此時的積分結果α、β、γ可以理解爲bk+1對bk的相對運動量，分別對應量綱爲位移，速度和四元數。這裏的α、β、γ可以用一階泰勒展開來獲取它們的近似值。
其中，帶有^的量是由IMU測量量直接計算得到的。dba和dbw是bias的變化量，J是它們與α、β、γ對應的Jacobian。所以說要想求α、β、γ，我們需要首先分別求出它們由IMU測量量直接計算得到的標量值，然後再用bias進行修正獲取真實值。
這一部分代碼，見src/vins_estimator/estimator_node.cpp的evaluate()函數。
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)
{//get IMU residuals Qi transform coordinate from i to w; Vi volosity at time i in w coordinate
Eigen::Matrix<double, 15, 1> residuals;
// O_P = 0,O_R = 3, O_V = 6, O_BA = 9, O_BG = 12
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);//0,9
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);//0,12
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);//3,12
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);//6,9
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);//6,12
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
//cuihuakun p6-(6) // IMU預積分的結果,消除掉acc bias和gyro bias的影響, 對應IMU model中的\hat{\alpha},\hat{\beta},\hat{\gamma}
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;
//ppt ch3-p69 // IMU項residual計算,輸入參數是狀態的估計值, 上面correct_delta_*是預積分值, 二者求'diff'得到residual.
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.1.2 離散形式
這一部分的目的是時刻求出從bk到bk+1時刻α尖，β尖、γ尖的值。從而α、β、γ提供一個初始值，等待後續被bias和相應的J進行修正。
如上圖所示，IMU的採樣頻率要比feature_tracker_node的圖像幀發佈頻率要高得多，分別對應着上圖的紅線和綠線。假設相鄰的兩根紅線對應着i和i+1時刻，那麼某一個delta_ti時間範圍內，它的平均加速度，平均角速度分別是：
那麼i+1時刻的PVQ分別是，
這個公式很明顯可以用程序迭代的方式來計算，從第bk到bk+1時刻的值都能用這種方式遞推獲取。這部分代碼在VINS裏出現過多次。
第一次出現，見src/vins_estimator/estimator_node.cpp的pridict()函數。
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{ //獲取當前時間
double t = imu_msg->header.stamp.toSec();
//首幀判斷
if (init_imu)
{latest_time = t;init_imu = 0;return;}
//獲取dt並傳遞時間
double dt = t - latest_time;
latest_time = t;
//獲取當前時刻的IMU採樣數據
double dx = imu_msg->linear_acceleration.x;
double dy = imu_msg->linear_acceleration.y;
double dz = imu_msg->linear_acceleration.z;
Eigen::Vector3d linear_acceleration{dx, dy, dz};
double rx = imu_msg->angular_velocity.x;
double ry = imu_msg->angular_velocity.y;
double rz = imu_msg->angular_velocity.z;
Eigen::Vector3d angular_velocity{rx, ry, rz};
//注意，以下數據都是世界座標系下的
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
tmp_V = tmp_V + dt * un_acc;
//信息傳遞
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
第二次出現，見src/vins_estimator/estimator.cpp的processIMU()函數，分析見5.1.2-2。
第三次出現，見vins_estimator/src/factor/integration_base.h的midPointIntegration()的前半段。
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)
{
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);//after dt:qi+1
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);//average mid
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;//alpha(i+1)
result_delta_v = delta_v + un_acc * _dt;//beta(i+1)
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
}
4.2 誤差傳遞矩陣
爲什麼要求誤差傳遞函數？主要有2個目的，第一個是在求α、β、γ的值的時候給不同bias提供相應的Jacobian；第二個目的是給後端優化部分中IMU部分提供信息矩陣。
實際上講，IMU在每一個時刻的測量都是有誤差的，而且這個誤差是滿足的高斯分佈的。在IMU預積分的遞推過程中，由於下一時刻的值是由上一時刻的值再加上當前的測量值獲得的，所以每一時刻的PVQ的計算值所對應的方差也是在不斷累積。因此，求出對應的誤差傳遞函數十分重要。
所以，根據誤差傳遞函數的定義，我們可以建立一個下面這個形式的誤差傳遞線性模型，它描述了下一時刻和當前時刻誤差的關係：
dzk+1和dzk分別是下一時刻的誤差和上一時刻的誤差，上面這個等式對應的向量/矩陣表達式如下式所示：
至於每一項的推導過程，請見參考文獻中提供的鏈接。
Jacobian的迭代公式爲：
這個J有了之後，帶入4.1.1最後那個公式中去，就能得到真正的α、β、γ。注意的是，VINS裏面出現過多次J的計算，一個是在這，另三個是在後端優化裏出現，注意不要混淆。
協方差迭代公式爲：
這個協方差矩陣，可以用於計算後端優化中IMU部分的信息矩陣。
噪聲項的對角協方差矩陣Q爲：
這一部分代碼對應在midPointIntegration()的後半段，如下：
if(update_jacobian)
{ //這些量都是給F的填充作輔助
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();
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;
jacobian = F * jacobian;//used for cuihuakun p6-(6)
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
4.3 IntegrationBase類的解讀
這部分內容主要出現在vins_estimator/src/factor/integration_base.h中。
4.3.1 構造函數的定義
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();//cuihuakun p9
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();
}
這個構造函數讀取了4個i時刻的初始值(共12個變量)，
用_acc_0初始化acc_0和linearized_acc(分別是i時刻和bk時刻)，
用_gyr_0初始化gyr_0和linearized_gyr(分別是i時刻和bk時刻)，
用1515的單位矩陣初始化J，
用1515的0矩陣初始化協方差傳遞矩陣。
用yaml文件裏讀取的IMU噪聲參數初始化18*18的噪聲矩陣。
4.3.2 傳播與重新傳播
對應的函數分別是propagate()和repropagate()。
對於propagate()而言，它的使用次數更多，針對的是bk和bk+1內部的i時刻到i+1時刻的PVQ傳播和誤差傳遞。
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1; //a at time t=t+dt
gyr_1 = _gyr_1; //w at time t=t+dt
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; //alpha_i+1
delta_q = result_delta_q; //gama_i+1 from i+1 coordinate to bk
delta_v = result_delta_v; //beta_i+1
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize();
sum_dt += dt; //time for bk to bk+1
acc_0 = acc_1;//a_i+1 at bi+1 coordinate
gyr_0 = gyr_1;//w_i+1
}
對於repropagate()而言，它的使用次數較少，針對的是從bk到bk+1的PVQ傳播矯正和誤差傳遞矯正。
void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
{
sum_dt = 0.0;//the gap between IMU plot
acc_0 = linearized_acc;//a at bk in bk coordinate
gyr_0 = linearized_gyr;//w at bk in bk coordinate
delta_p.setZero();//alpha
delta_q.setIdentity();//gama trans bi to bk
delta_v.setZero();//beta
linearized_ba = _linearized_ba;
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]);
}