来一个很有意思的ESKF吧

0. 简介

作者之前对KF,EKF,UKF,PF都进行了学习，但是有两块KF还没有进行精细的学习，而相较于IEKF而言，ESKF会在滤波和融合定位中更常使用，当然学习了KF后，对于其他的变种卡尔曼滤波理解起来会非常容易，基本上问题不是很大。状态误差卡尔曼(ESKF)的应用，它是卡尔曼滤波器的变种中应用最为广泛的一种，与EKF一样，它也是一种针对时变系统的非线性滤波器。但是与EKF不同的是，它的线性化是总是在0附近，因此线性化更准确。绝大部分的场景，ESKF就足够使用了。如果对于滤波有更高的要求，可以选择UKF，甚至PF。

1. ESKF介绍

Error-state Kalman Filter（ESKF）就是一种传感器融合的算法，它的基础仍然是卡尔曼滤波。它的核心思想是把系统的状态分为三类：

• true state：实际状态，系统实际的运行状态
• nominal state：名义状态，描述了运动状态的主要趋势，主导成分。（large-signal，非线性）
• error state：误差状态，实际状态与名义状态之间的差值（small-signal，近似线性，适合线性高斯滤波）

基于以上状态分类，我们可以将关心的true-state，分为两部分，分别进行估计，即nominal state和error state，然后再进行二者叠加。 ESKF的全过程可以描述为如下步骤：

• 对高频率的IMU数据$u$ 进行积分，获得nomimal state $x$。nominal state里面不考虑噪声，因此势必会累积误差。这部分误差可依据error state进行修正。
• 利用Kalman Filter估计error state，这里同样也包括时间更新和量测更新两部分。这个过程考虑了噪声，并且由于误差状态方程是近似线性的，所以可以直接用卡尔曼滤波。
• 利用error state修正nominal state，获取true state。
• 重置error state

使用ESKF的优势：

• error state 中的参数数量与运动自由度是相等的，避免了过参数化（over-parameterization or redundancy）引起的协方差矩阵奇异的风险。
• error state 总是接近于0，Kalman Filter工作在原点附近。因此，远离奇异值、万向节锁，并且保证了线性化的合理性和有效性。
• error state 总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速。
• error state 的变化平缓，因此KF修正的频率不需要太高。

1.1 Nominal State 构建

这部分只要是自更新，将IMU的所有状态量成分进行提取，并构建出一个Nominal state的动力学模型，且这个模型是不受噪声分量影响的，因为噪声分量的影响都放到了Error-state的动力学模型中。

$p \leftarrow p+v \Delta t+ \frac {1}{2} (R( a_ {m} - a_ {b} )+g) \Delta t^ {2} \\ v \leftarrow v+(R( a_ {m} -a_ {b} +g) \Delta t \\ q \leftarrow q \otimes q \{( \omega _ {m} - \omega _ {b} ) \Delta t\} \\ a_ {b} \leftarrow a_ {b} \\ \omega _ {b} \leftarrow \omega _ {b} \\ g \leftarrow g$
其中
$q \{ v \} = e^{v/2}$
也就意味着
$q \{( \omega _ {m} - \omega _ {b} ) \Delta t\} = e^{(w_m-w_b)\Delta t/2} = \left[ \begin{matrix} \cos(\theta/2)\\u\sin(\theta/2)\end{matrix} \right] \\ \theta =||( \omega _ {m} - \omega _ {b} ) \Delta t|| ,u= \frac {(\omega_m-\omega_b)\Delta t}{\theta }$

void ESKF_update_nominal(ImuDataPtr imu_data,double dt, State* state, Eigen::Vector3d g){
    double dt2_2 = 0.5*dt*dt;
    State old_state = *state;

    state->G_p_I = old_state.G_p_I + dt * old_state.G_v_I + dt2_2 * (old_state.G_q_I * (imu_data->accel - old_state.ab) + g);
    state->G_v_I = old_state.G_v_I + dt * (old_state.G_q_I * (imu_data->accel - old_state.ab) + g);

    if (imu_data->quat.norm() != 0 && imu_data->last_quat.norm() != 0){
      //quaternion valid, update orientation according to delta quaternion
      Eigen::Matrix3d d_rot(imu_data->last_quat.inverse() * imu_data->quat);

      state->G_q_I = old_state.G_q_I * d_rot;
      const Eigen::Vector3d d_theta = (- old_state.wb) * dt;
      if (d_theta.norm() >= 1e-12){
        Eigen::AngleAxisd d_rot2(d_theta.norm(),d_theta.normalized());
        state->G_q_I *= d_rot2.toRotationMatrix();
      }
    }else{
      const Eigen::Vector3d d_theta = (imu_data->gyro - old_state.wb) * dt;
      if (d_theta.norm() >= 1e-12){
        Eigen::AngleAxisd d_rot(d_theta.norm(),d_theta.normalized());
        state->G_q_I = old_state.G_q_I * d_rot.toRotationMatrix();
      }
    }

    if(imu_data->quat.norm() != 0){
      //current quaternion valid, haven't got last quaternion.
      imu_data->last_quat = imu_data->quat;
    }

    //state->ab = old_state.ab;
    //state->wb = old_state.wb;
  }

2. Error-state 状态构建

然后就是根据当前状态进行获得含有累计误差的方程，来用于获取误差状态量更新和协方差矩阵的累计更新。

定义状态向量：

于是误差状态方程可以写作：

其中:

定义：

这是一个微分方程的闭合解，利用这个解进行递推会更精确。当然，也可以只取前面两项。

然后就可以完成误差姿态的更新，(这部分的累加看公式来说，并没有起到作用)

协方差矩阵更新

​