惯性导航(三)-基于流型的ESKF及代码实现

0. 专栏导读

惯性导航在SLAM技术的应用中有着举足轻重的作用,从刚体的姿态解算,到今年来常用的多传感器融合都离不开惯性导航,而惯导又涉及很多的理论知识,为此开设本专栏的旨在汇总惯性导航的知识点.
专栏涵盖以下几个方面:

1. 从KF到EKF;
2. 姿态解算;
3. 基于流型(李代数)的ESKF及代码实现(本文);
4. 预积分(待发布).

1. 引言

1.1 误差卡尔曼滤波

误差卡尔曼滤波(Error State Kalman Filter, ESKF)虽然从简称上看和EKF相似,但实质上还是有很大区别.无论KF还是EKF所关心的状态本身,而ESKF将注意力转到状态的误差,称为误差状态变量(Error State),而原来的状态称为名义状态比变量(Nominal State),名义状态变量和误差状态变量之和称为真值.

$${\mathbf{x}}_t=\mathbf{x}+\delta \mathbf{x}\text{\hspace{1em}(1.1)}$$
上式中:

• ${\mathbf{x}}_t$: 真值,右下角的t是指true,而不是时间
• $\mathbf{x}$: 名义状态量
• $\delta \mathbf{x}$: 误差状态量

名义状态量不必考虑噪声,噪声在误差状态中考虑了.

1.2 ESKF的特点

相比传统的EKF,ESKF有以下优势:

• ESKF总是在原点附近,离奇异点较远,数值方面更稳定,并且不会产生离工作点太远而导致线性化近似不够的问题;
• ESKF的状态量为小量,其二阶变量相对来说可以忽略;同时,大多数雅可比矩阵在小量情况下变得非常简单,甚至可以用单位阵代替.

2. 数学推导

惯导系统中我们关心的状态有平移/速度/旋转,IMU的陀螺仪和加速度零偏服从随机游走模型,因此将其加入到状态中一并估计,另外也可以将重力g也放入到状态中(方便确定IMU的初始位姿,否则额需要实现确定初始时刻的IMU朝向),因此设状态变量如下:

$$\mathbf{x}=[\mathbf{p},\mathbf{v},\mathbf{q},{\mathbf{b}}_g,{\mathbf{b}}_a,\mathbf{g}{]}^T\text{\hspace{1em}(2.1)}$$

根据误差状态的定义有:
$$\begin{array}{rl}& p_t=p+\delta \mathbf{p}\\ & {\mathbf{v}}_t=\mathbf{v}+\delta \mathbf{v}\\ & R_t=\mathbf{R}\delta \mathbf{R}\text{or }{\mathbf{q}}_t=\mathbf{q}\delta \mathbf{q}\\ & {\mathbf{b}}_{gt}={\mathbf{b}}_g+\delta {\mathbf{b}}_g\\ & {\mathbf{b}}_{at}={\mathbf{b}}_a+\delta {\mathbf{b}}_a\\ & g_t=g+\delta \mathbf{g}\end{array}\text{\hspace{1em}(2.2)}$$

2.1 运动过程

和EKF一样,ESKF通过运动方程由上一时刻状态估计当前时刻,由于ESKF涉及误差状态和名义状态,因此不但需要知道名义状态的递推关系,还需要根据式2.2的定义推导误差状态的递推关系.

2.1.1 名义状态预测

首先处理名义状态对时间的导数,具体如下:
$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a)+\mathbf{g}\\ \dot{\mathbf{R}}& =\mathbf{R}(\tilde{\mathbf{w}}-{\mathbf{b}}_g{)}^{\wedge }\\ {\dot{\mathbf{b}}}_a& ={\eta }_{ba}\\ {\dot{\mathbf{b}}}_g& ={\eta }_{bg}\\ \dot{\mathbf{g}}& =0\end{array}\text{\hspace{1em}(2.3)}$$
由上面的时间导数以得离散时间的运动方程:
$$\begin{array}{rl}& \mathbf{p}(t+\Delta t)=\mathbf{p}(t)+\mathbf{v}\Delta t+\frac{1}{2}(\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a))\Delta t^2+\frac{1}{2}\mathbf{g}\Delta t^2\\ & \mathbf{v}(t+\Delta t)=\mathbf{v}(t)+\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a)\Delta t+\mathbf{g}\Delta t\\ & \mathbf{R}(t+\Delta t)=\mathbf{R}(t)\mathrm{Exp}\left((\tilde{\mathbf{\omega }}-{\mathbf{b}}_g)\Delta t\right)\\ & {\mathbf{b}}_g(t+\Delta t)={\mathbf{b}}_g(t)\\ & {\mathbf{b}}_a(t+\Delta t)={\mathbf{b}}_a(t)\\ & \mathbf{g}(t+\Delta t)=\mathbf{g}(t)\end{array}\text{\hspace{1em}(2.4)}$$

2.1.2 误差状态预测

根据误差状态的定义可求得其连续时间的导数:
$$\begin{array}{rl}\delta \dot{p}& =\delta \mathbf{v}\\ \delta \dot{v}& =-\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a{)}^{\wedge }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{b}}_a-{\mathbf{\eta }}_a+\delta \mathbf{g}\\ \delta \dot{\theta }& =-(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g{)}^{\wedge }\delta \mathbf{\theta }-\delta {\mathbf{b}}_g-{\mathbf{\eta }}_g\\ \delta {\dot{b}}_g& ={\eta }_{bg}\\ \delta {\dot{b}}_a& ={\eta }_{ba}\\ \delta \dot{g}& =0\end{array}\text{\hspace{1em}(2.5)}$$
上式的推导只需明确以下两点:

• 根据误差定义式2.2分别对两边求时间导数;
• 名义状态和真值相同,只是不必考虑噪声

其中速度 $v$ 和旋转 $\theta$ 的推导过程较为复杂,需要用到SO3的伴随性质,详见高博的推导或书籍<自动驾驶与机器人中的SLAM技术-惯性导航与组合导航>
同理还需要求误差状态离散时间下的递推关系:

$$\begin{array}{rl}& \delta \mathbf{p}(t+\Delta t)=\delta \mathbf{p}+\delta \mathbf{v}\Delta t\\ & \delta \mathbf{v}(t+\Delta t)=\delta \mathbf{v}+\left(-\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a{)}^{\wedge }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{b}}_a+\delta \mathbf{g}\right)\Delta t+{\mathbf{\eta }}_v\\ & \delta \mathbf{\theta }(t+\Delta t)=\mathrm{Exp}\left(-(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g)\Delta t\right)\delta \mathbf{\theta }-\delta {\mathbf{b}}_g\Delta t-{\mathbf{\eta }}_{\theta }\\ & \delta {\mathbf{b}}_g(t+\Delta t)=\delta {\mathbf{b}}_g+{\mathbf{\eta }}_g\\ & \delta {\mathbf{b}}_a(t+\Delta t)=\delta {\mathbf{b}}_a+{\mathbf{\eta }}_a\\ & \delta \mathbf{g}(t+\Delta t)=\delta \mathbf{g}\end{array}\text{\hspace{1em}(2.6)}$$

回顾EKF,我们需要知道待估计状态在前一时刻到当前时刻的转换矩阵,而式2.6正是反映误差状态的转换关系,因此对于这里的ESKF而言,运动方程的雅可比矩阵为:

$$F=\left[\begin{array}{cccccc}I& I\Delta t& 0& 0& 0& 0\\ 0& I& -\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a{)}^{\wedge }\Delta t& 0& -\mathbf{R}\Delta t& I\Delta t\\ 0& 0& \mathrm{Exp}\left(-(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g)\Delta t\right)& -\mathbf{I}\Delta t& 0& 0\\ 0& 0& 0& I& 0& 0\\ 0& 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0& I\end{array}\right]\text{\hspace{1em}(2.7)}$$

为此,ESKF对误差状态的预测如下:
$$\begin{array}{rl}\delta {\mathbf{x}}_{pred}& =\mathbf{F}\delta \mathbf{x}\\ {\mathbf{P}}_{pred}& =\mathbf{F}\mathbf{P}{\mathbf{F}}^T+\mathbf{Q}\end{array}\text{\hspace{1em}(2.8a)(2.8b)}$$
式2.8a实则意义不大,因为ESKF在误差状态每次更新以后会被重置 $\delta \mathbf{x}=0$.因此预测过程主要是计算误差状态协方差.

2.2 更新过程

传统EKF中,根据观测方程的雅可比矩阵结合贝叶斯公式计算状态的后验.而ESKF中误差状态的后验直接对应名义状态的更新量,即
$$\begin{array}{rl}\delta \mathbf{x}& =\mathbf{K}\left(\mathbf{z}-h(\mathbf{x})\right)\\ \mathbf{x}& ={\mathbf{x}}_{pred}+\delta \mathbf{x}\end{array}\text{\hspace{1em}(2.9a)(2.9b)}$$
其中 $K$为增益:
$$\mathbf{K}={\mathbf{P}}_{pred}{\mathbf{H}}^T(\mathbf{H}{\mathbf{P}}_{pred}{\mathbf{H}}^T+\mathbf{V}{)}^{-1}\text{\hspace{1em}(2.10)}$$
误差状态的协方差更新为:
$$\mathbf{P}=(\mathbf{I}-\mathbf{K}\mathbf{H}){\mathbf{P}}_{pred}\text{\hspace{1em}(2.11)}$$

不难发现,上面式2.9-式2.11其实和传统的EKF的更新过程是完全一致,名义状态 $\mathbf{x}$ 的调整量实质就是 $\delta \mathbf{x}$, 但是,这里 $\mathbf{H}$ 不再是观测方程的 $h$ 关于 $\mathbf{x}$ 的雅可比矩阵,而是关于 $\delta \mathbf{x}$ 的雅可比,通过链式法则有:
$$\mathbf{H}=\frac{\partial h}{\partial \delta \mathbf{x}}=\frac{\partial h}{\partial \mathbf{x}}\frac{\partial \mathbf{x}}{\partial \delta \mathbf{x}}\text{\hspace{1em}(2.12)}$$
第一项和观测量有关,只需对观测方程线性化即可;第二项根据名义状态与误差状态定义(式2.2)不能得出:
$$\frac{\partial \mathbf{x}}{\partial \delta \mathbf{x}}=diag({\mathbf{I}}_3,{\mathbf{I}}_3,\frac{\partial Log(\mathbf{R}Exp(\delta \theta ))}{\partial \delta \theta },{\mathbf{I}}_3,{\mathbf{I}}_3,{\mathbf{I}}_3)\text{\hspace{1em}(2.13)}$$
使用BCH近似可得:
$$\frac{\partial Log(\mathbf{R}Exp(\delta \theta ))}{\partial \delta \theta }={\mathbf{J}}_r^{-1}(\mathbf{R})\text{\hspace{1em}(2.14)}$$

2.3 重置(option)

ESKF相比传统EKF还需要进行重置操作,这是因为误差状态的均值每一时刻结束时都被叠加到名义状态上(即式2.9b),对于下一时刻误差状态的先验均值应为0.引用高博的话说:

重置前,卡尔曼滤波刻画了 ${\mathbf{x}}_{pred}$ 切空间处的分布 $N(\delta \mathbf{x},\mathbf{P})$, 重置后刻画的是 ${\mathbf{x}}_{pred}+\delta \mathbf{x}$ 处的分布 $N(0,{\mathbf{P}}_{reset})$.对于本是是矢量的状态是无差的,但对于旋转变量来说,它们的切空间零点发生了改变,所以在数学习惯上,需要进行区分

用数学表达上面的过程如下:
$${\mathbf{R}}^{+}Exp(\delta {\mathbf{\theta }}^{+})={\mathbf{R}}_{k}Exp(\delta {\mathbf{\theta }}_k)Exp(\delta {\mathbf{\theta }}^{+})={\mathbf{R}}_{k}Exp(\delta \mathbf{\theta })\text{\hspace{1em}(2.15)}$$
上式中:

• ${\mathbf{R}}_k$: 重置前的名义旋转状态;
• $\delta \mathbf{\theta }$: 重置前的误差旋转状态;
• $\delta {\mathbf{\theta }}_k$: 卡尔曼滤波更新后的误差旋转量;
• ${\mathbf{R}}^{+}$: 重置后的名义旋转状态;
• $\delta {\mathbf{\theta }}^{+}$: 重置后的误差旋转状态.

切空间零点变化需要调整协方差,这里调整的方法和EKF运动方程/观测方程线性化的思想类似,都是通过泰勒展开近似成线性关系,重置前的"不确定性"通过该线性关系传递到重置后,这里的推导使用了BCH公式,这里同样不再详述这个过程,直接给出结论:
$$\begin{array}{rl}{\mathbf{P}}_{reset}& ={\mathbf{J}}_k\mathbf{P}{\mathbf{J}}_k^T\\ {\mathbf{J}}_k& =diag({\mathbf{I}}_3,{\mathbf{I}}_3,{\mathbf{J}}_{\theta },{\mathbf{I}}_3,{\mathbf{I}}_3,{\mathbf{I}}_3)\\ {\mathbf{J}}_{\theta }& =\frac{\partial \delta {\mathbf{\theta }}^{+}}{\partial \delta \mathbf{\theta }}=\mathbf{I}-\frac{1}{2}\delta {\mathbf{\theta }}_k^{\wedge }\end{array}\text{\hspace{1em}(2.16)}$$

注意:

由于 $\delta {\mathbf{\theta }}_k$ 并不大, ${\mathbf{J}}_k$十分接近单位矩阵,所以大部分的材料并不处理这一项,而是直接把前面估计的 $\mathbf{P}$ 矩阵作为下一时刻的协方差.

3. 代码实现

ESKF的C++实现可参考高博的<自动驾驶与机器人中的SLAM技术>书籍的配套源码(https://github.com/gaoxiang12/slam_in_autonomous_driving.git),下面对源码各部分对照上面数学推导作分析.

3.1 预测

template <typename S>
bool ESKF<S>::Predict(const IMU& imu) {
    ...
    // 对应式2.4
    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;

    // 对应式2.7
    // F实际上是稀疏矩阵，也可以不用矩阵形式进行相乘而是写成散装形式，这里为了教学方便，使用矩阵形式
    Mat18T F = Mat18T::Identity();                                                 // 主对角线
    F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;                         // p 对 v
    F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;  // v对theta
    F.template block<3, 3>(3, 12) = -R_.matrix() * dt;                             // v 对 ba
    F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;                        // v 对 g
    F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();     // theta 对 theta
    F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;                        // theta 对 bg

    // 对应式2.8a
    dx_ = F * dx_;  // 这行其实没必要算，dx_在重置之后应该为零，因此这步可以跳过，但F需要参与Cov部分计算，所以保留
    // 对应式2.8b
    cov_ = F * cov_.eval() * F.transpose() + Q_;
    ...
}

3.2 更新

以轮式编码器计算速度作为观测方程,更新过程如下:

template <typename S>
bool ESKF<S>::ObserveWheelSpeed(const Odom& odom) {
    assert(odom.timestamp_ >= current_time_);
    // 对应式2.12
    // 使用三维的轮速观测,由于这里观测量不涉及旋转,所以H较为简单，H为3x18，大部分为零
    Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();
    H.template block<3, 3>(0, 3) = Mat3T::Identity();

    // 对应式2.10 卡尔曼增益
    Eigen::Matrix<S, 18, 3> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + odom_noise_).inverse();

    // compute velocity
    VecT vel_world = ...

    // 对应式2.9a
    dx_ = K * (vel_world - v_);

    // 对应式2.11 update cov
    cov_ = (Mat18T::Identity() - K * H) * cov_;

    UpdateAndReset();
    ...
}

void UpdateAndReset() {
    // 对应式2.9b
    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);

    ...
}

3.3 重置

重置协方差,对应式2.16

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

4. 小结

本文从理论到代码实现介绍了ESKF相关内容,整体而言,ESKF与传统的EKF有以下区别:

1. 引入了误差状态和名义状态,名义状态可以理解为不考虑噪声的状态,真值与名义状态相差的正是误差状态;
2. 运动预测过程除了预测名义状态外,还需要根据误差状态的定义计算其递推过程并预测;
3. 观测过程的雅可比矩阵是关于误差状态本身的,而不是名义状态;
4. 除了传统EKF的预测和更新两个步骤外,ESKF还需要进行重置操作,重置的目的是流型空间下状态的更新会影响其协方差,但是具体实现也是可以忽略.

另外，上面的旋转部分是以李代数表示并推导,实际上同样可以使用四元数表示并推导,日后再补充至本专栏.