VIO中的IMU積分
1、數值積分原理
對於一個給定的微分方程 ，假設已經知道了初值 ，則其 時刻後的數值積分爲：node
實際當中咱們一般沒法得到 的表達式，只能對其進行離散採樣，而後使用離散積分逼近真實的連續積分。一般近似的思路爲：認爲 到 的微分 用恆定常數來表示，則積分表達式可改寫爲：web
從上述公式能夠看出，減小積分偏差的主要方式有兩種：微信
1. 減小採樣時間間隔 ，時間間隔越小，則離散積分就越接近於連續積分，採樣時間間隔取決於sensor的採樣率，因此採樣率越高積分精度越高；app
2. 計算精確的恆定常數 ，針對 的一般有三種積分方法：歐拉積分、中值積分和4階龍格-庫塔積分。編輯器
2、積分方法
2.1 歐拉積分
歐拉積分假設在倒數區間內的斜率是恆定的，其取 時刻的斜率做爲 至 時間段的斜率，即：ide
從公式能夠看出，歐拉積分是最簡單的一種積分方式，其逼近偏差較大，但計算量很小。svg
2.2 中值積分
中值積分是在歐拉積分的基礎上進行改善。先使用歐拉積分逼近時間間隔 的中點，即 的斜率，而後使用中點斜率做爲整個時間段內的近似斜率。ui
首先使用歐拉積分來近似時間段內的中點斜率：url
而後咱們使用獲得的時間段中點斜率進一步近似整個時間段的斜率：spa
顯而易見，中值積分比歐拉積分更合理一些。
2.2 4階龍格-庫塔積分
咱們直接給出4階龍格-庫塔積分的公式：
是起點的斜率， 是中點的斜率， 是中點的中點斜率， 是終點的斜率。實際上4階龍格-庫塔積分就是斜率的加權結果， 與 的斜率權重爲2，其他爲1。顯而易見，這種方法的近似精度是最高的。其中 就是歐拉積分當中的斜率， 就是中值積分當中的斜率。
4階龍格-庫塔積分的代碼以下：
//Indirect Kalaman Filter for 3D Attitude Estimation的第11頁
// Note we use Vector4f to represent quaternion instead of quaternion
// because it is better do not normalize q during integration
inline Eigen::Vector4f XpQuaternionDerivative(
const Eigen::Vector4f &q,
const Eigen::Vector3f &omega) {
return -0.5 * XpComposeOmega(omega) * q;
}
//k時刻角速度,k+1時刻角速度,k時刻旋轉,dalta_t,k+1時刻旋轉
void IntegrateQuaternion(
const Eigen::Vector3f &omega_begin, const Eigen::Vector3f &omega_end,
const XpQuaternion &q_begin, const float dt,
XpQuaternion *q_end,
XpQuaternion *q_mid,
XpQuaternion *q_fourth,
XpQuaternion *q_eighth) {
CHECK_NOTNULL(q_end);
XpQuaternion q_0 = q_begin;
// divide dt time interval into num_segment segments
// TODO(mingyu): Reduce to 2 or 4 segments as 8 segments may overkill
const int num_segment = 8;//步數
// the time duration in each segment
const float inv_num_segment = 1.0 / num_segment;//步長
const float dt_seg = dt * inv_num_segment;
Eigen::Vector3f delta_omega = omega_end - omega_begin;
for (int i = 0; i < num_segment; ++i) {
// integrate in the region: [i/num_segment, (i+1)/num_segment]
//這裏利用陀螺儀的測量RK4積分算出先後幀一個imu測量出的旋轉,JPL格式四元數
// 那麼XpQuaternionDerivative(q_0, omega_tmp)應該是 - 0.5 * omega(w) × qcur_pre(t)
Eigen::Vector3f omega_tmp = omega_begin + (i * inv_num_segment) * delta_omega;//當前步長內的imu角速度
Eigen::Vector4f k1 = XpQuaternionDerivative(q_0, omega_tmp);
omega_tmp = omega_begin + 0.5 * (2 * i + 1) * inv_num_segment * delta_omega;
Eigen::Vector4f k2 = XpQuaternionDerivative(q_0 + 0.5 * k1 * dt_seg, omega_tmp);
Eigen::Vector4f k3 = XpQuaternionDerivative(q_0 + 0.5 * k2 * dt_seg, omega_tmp);
omega_tmp = omega_begin + (i + 1) * inv_num_segment * delta_omega;
Eigen::Vector4f k4 = XpQuaternionDerivative(q_0 + k3 * dt_seg, omega_tmp);
(*q_end) = q_0 + (dt_seg / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
// store the start point the next region of integration
q_0 = (*q_end);
// store the intermediate attitude as output
if (i == 0 && q_eighth) {
(*q_eighth) = (*q_end);
} else if (i == 1 && q_fourth) {
(*q_fourth) = (*q_end);
} else if (i == 3 && q_mid) {
(*q_mid) = (*q_end);
}
}
}
SLAM標定系列文章
本文分享自微信公衆號 - 科學計算Tech（Quant_Times）。
若有侵權，請聯繫 support@oschina.cn 刪除。
本文參與「OSC源創計劃」，歡迎正在閱讀的你也加入，一塊兒分享。