ESKF 總結(jié)

prediction

更新誤差先驗

$F$通過3.42來算

得到

這里有點繞的一點是: 誤差狀態(tài)的$F$牽涉到名義狀態(tài), 而名義狀態(tài)又需要在時間上推進更新

其中, F中的名義狀態(tài)的推進通過公式3.41得到,

(名義狀態(tài)不考慮誤差, 這一點從3.41d, 3.41e可以看出, 誤差狀態(tài)只考慮誤差, 真實狀態(tài)為名義+誤差)

代碼的實現(xiàn)

template <typename S>
bool ESKF<S>::Predict(const IMU& imu) {assert(imu.timestamp_ >= current_time_);double dt = imu.timestamp_ - current_time_;if (dt > (5 * options_.imu_dt_) || dt < 0) {// 時間間隔不對，可能是第一個IMU數(shù)據(jù)，沒有歷史信息LOG(INFO) << "skip this imu because dt_ = " << dt;current_time_ = imu.timestamp_;return false;}// nominal state 遞推VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);R_ = new_R;v_ = new_v;p_ = new_p;// 其余狀態(tài)維度不變// error state 遞推// 計算運動過程雅可比矩陣 F，見(3.47)// F實際上是稀疏矩陣，也可以不用矩陣形式進行相乘而是寫成散裝形式，這里為了教學(xué)方便，使用矩陣形式Mat18T F = Mat18T::Identity();                                                 // 主對角線F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;                         // p 對 vF.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;  // v對thetaF.template block<3, 3>(3, 12) = -R_.matrix() * dt;                             // v 對 baF.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;                        // v 對 gF.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();     // theta 對 thetaF.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;                        // theta 對 bg// mean and cov predictiondx_ = F * dx_;  // 這行其實沒必要算，dx_在重置之后應(yīng)該為零，因此這步可以跳過，但F需要參與Cov部分計算，所以保留cov_ = F * cov_.eval() * F.transpose() + Q_; current_time_ = imu.timestamp_;return true;
}

$Q$的算法

注意, 在離散情況下
$\begin{array}{rl}& {\eta }_v={\eta }_a\Delta t\\ & {\eta }_{\theta }={\eta }_g\Delta t\\ & {\eta }_g={\eta }_{bg}\Delta t\\ & {\eta }_a={\eta }_{ba}\Delta t\end{array}$
可以根據(jù)3.40將3.42進行一階泰勒展開

代碼中的實現(xiàn)

        double ev = options.acce_var_;double et = options.gyro_var_;double eg = options.bias_gyro_var_;double ea = options.bias_acce_var_;double ev2 = ev;  // * ev;   // tj : 為什么沒平方? Q里面應(yīng)該是方差double et2 = et;  // * et;double eg2 = eg;  // * eg;double ea2 = ea;  // * ea;// 設(shè)置過程噪聲Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;

其中options的定義

    struct Options {Options() = default;/// IMU 測量與零偏參數(shù)double imu_dt_ = 0.01;  // IMU測量間隔// NOTE IMU噪聲項都為離散時間，不需要再乘dt，可以由初始化器指定IMU噪聲double gyro_var_ = 1e-5;       // 陀螺測量標(biāo)準(zhǔn)差double acce_var_ = 1e-2;       // 加計測量標(biāo)準(zhǔn)差double bias_gyro_var_ = 1e-6;  // 陀螺零偏游走標(biāo)準(zhǔn)差double bias_acce_var_ = 1e-4;  // 加計零偏游走標(biāo)準(zhǔn)差

correction

更新誤差后驗

$H$是觀測值對誤差狀態(tài)變量的jacob

先看GNSS, 它觀測值有位移和旋轉(zhuǎn), 根據(jù)3.66和3.70可得$H$,

GNSS對旋轉(zhuǎn)的觀測是總體的旋轉(zhuǎn), 然而名義上的$R$是知道的, 所以我們可以將觀測值變以成對于旋轉(zhuǎn)誤差狀態(tài)的直接觀測, 這樣一來, 它對自己求導(dǎo)就為$I$

同理, GNSS對位移的觀測是總體的位移, 然后名義上的$p$是知道, 所以我們將對總體位移的觀測轉(zhuǎn)到對于誤差位移的直接觀測

    Eigen::Matrix<S, 6, 18> H = Eigen::Matrix<S, 6, 18>::Zero();H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分 (3.70)H.template block<3, 3>(3, 6) = Mat3T::Identity();  // R部分（3.66)

然后求$V$

 // 卡爾曼增益和更新過程Vec6d noise_vec;noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;Mat6d V = noise_vec.asDiagonal();

其中trans_noise, ang_noise在option中定義

        /// RTK 觀測參數(shù)double gnss_pos_noise_ = 0.1;                   // GNSS位置噪聲double gnss_height_noise_ = 0.1;                // GNSS高度噪聲double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS旋轉(zhuǎn)噪聲

然后更新后驗3.51b, 3.51d

    // 更新x和covVec6d innov = Vec6d::Zero();innov.template head<3>() = (pose.translation() - p_);          // 平移部分innov.template tail<3>() = (R_.inverse() * pose.so3()).log();  // 旋轉(zhuǎn)部分(3.67)dx_ = K * innov;cov_ = (Mat18T::Identity() - K * H) * cov_;

有了誤差狀態(tài), 就可以更新名義狀態(tài)3.51c

   /// 更新名義狀態(tài)變量，重置error statevoid UpdateAndReset() {p_ += dx_.template block<3, 1>(0, 0);v_ += dx_.template block<3, 1>(3, 0);R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));if (options_.update_bias_gyro_) {bg_ += dx_.template block<3, 1>(9, 0);}if (options_.update_bias_acce_) {ba_ += dx_.template block<3, 1>(12, 0);}g_ += dx_.template block<3, 1>(15, 0);ProjectCov();dx_.setZero();}

注意 這里有一步需要投影, 參考3.63

/// 對P陣進行投影，參考式(3.63)
void ProjectCov() {Mat18T J = Mat18T::Identity();J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));cov_ = J * cov_ * J.transpose();
}

再看odom, 它觀測值只有速度

首先求H, 注意這里觀測值為本地速度, 但是名義變量$R$這時是知道的, 所以可以把觀測值從本地速度轉(zhuǎn)化到世界速度, 然后世界速度對世界速度求導(dǎo)就為$I$

// odom 修正以及雅可比
// 使用三維的輪速觀測，H為3x18，大部分為零
Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();
H.template block<3, 3>(0, 3) = Mat3T::Identity();

然后算V

// 設(shè)置里程計噪聲
double o2 = options_.odom_var_ * options_.odom_var_;
odom_noise_.diagonal() << o2, o2, o2;

option里面的定義

/// 里程計參數(shù)
double odom_var_ = 0.5;
double odom_span_ = 0.1;        // 里程計測量間隔
double wheel_radius_ = 0.155;   // 輪子半徑
double circle_pulse_ = 1024.0;  // 編碼器每圈脈沖數(shù)

然后更新后驗

本地速度觀測值的算法參見 3.76

// velocity obs
double velo_l = options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
double velo_r =options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
double average_vel = 0.5 * (velo_l + velo_r);VecT vel_odom(average_vel, 0.0, 0.0);
VecT vel_world = R_ * vel_odom;dx_ = K * (vel_world - v_);// update cov
cov_ = (Mat18T::Identity() - K * H) * cov_;
UpdateAndReset();