卡尔曼滤波在SLAM中的应用

卡尔曼知识补充说明

典型非线性卡尔曼(EKF)主要知识回顾

卡尔曼滤波是对输入线性系统的观测值进行最优估计的一种递归算法，在数据融合、滤波、预测等方面具有广泛的应用。我们无法建立一个完美的模型来消除系统的不确定因素，这些不确定性包括系统模型的自身缺陷、系统过程扰动（不可测也不可建模）以及得到观测数据的传感器的测量误差。这些误差往往会影响我们的估计值，所以卡尔曼滤波算法通过最小化误差方差求解卡尔曼增益，以此来进行后验估计，已达到滤波效果。卡尔曼滤波使用条件：有限维的线性系统，测量噪声和过程噪声均服从正态分布。

系统描述（已知条件）

1. 系统的状态方程（预测方程）
   $$x_k=A_k{x}_{k-1}+B_k{u}_k+w_k$$
2. 观测方程
   $$z_k=H_k{x}_k+v_k$$
   其中：
   $x_k、x_{k-1}$ ：当前和上一时刻状态向量，即所求目标变量s；
   $A_k$：状态转移矩阵，表示向量时刻状态之间的转换关系；
   $u_k$：状态的控制向量，即状态向量的因变量；
   $B_k$：控制变量矩阵，即控制变量与状态向量之间的相互作用关系；
   $w_k$：系统的噪声，服从高斯分布：$w_k\sim N(0,Q)$；
   $Q$：表示系统噪声的协防差矩阵；
   $z_k$：表示测量向量或者观测向量；
   $H_k$：表示状态向量与观测向量之间的转换矩阵；
   $v_k$：表示测量观测过程中的噪声，多指传感器测量误差，服从高斯分布$v_k\sim N(0,R)$；
   $R$：测量噪声的协方差矩阵；

五个核心公式

预测（利用前一时刻已知的信息估计下一时刻的先验状态向量和相关信息）：
$$\begin{gathered} {\hat{x}}_{\bar{k}}=A_k{\hat{x}}_{k-1}+B_k{u}_k \\ {P}_{\bar{k}}=A_k{P}_{k-1}{A}_k^T+Q \end{gathered}$$
状态更新（利用以后信息进一步修正状态向量和对应的协方差矩阵信息）：
$$\begin{gathered} K_k=\frac{P_{\bar{k}}{H}_k^T}{H_k{P}_{\bar{k}}{H}_k^T+R} \\ {\hat{x}}_k={\hat{x}}_{\bar{k}}+K_k(z_k-H_k{\hat{x}}_{\bar{k})} \\ {P}_k=(I-K_k{H}_k)P_{\bar{k}} \end{gathered}$$

其中：

1. ${\hat{x}}_{k-1}$ 和 ${\hat{x}}_k$分别为k-1时刻与k时刻的后验状态估计，是滤波结果之一，即更新后的结果，也叫最优估计。（因为不可能知道真实值，所以加一个hat，表示最优估计）；
2. ${\hat{x}}_{\bar{k}}$表示k时刻的的先验状态估计，及根据k-1时刻的最优估计预测k时刻的状态；
3. $P_{k-1}$ 和 $P_k$分别为k-1时刻与k时刻的后验估计协方差（即${\hat{x}}_{k-1}$ 和 ${\hat{x}}_k$ 的协方差，表示状态的不确定性），为滤波结果之一；
4. $P_{\bar{k}}$为k时刻的先验估计协方差（${\hat{x}}_{\bar{k}}$的协方差）；
5. $K_k$为卡尔曼增益。

核心推导过程

预测

定理1：随机变量函数的分布(高斯分布)
已知X 服从高斯分布：X $\sim$ N ( μ , ${\sigma }^2$ ) ，且Y = a X + b 。则Y 服从高斯分布：Y $\sim$ N ( a$\mu$ + b , $a^2{\sigma }^2$ )。
已知k-1时刻系统的最优状态服从高斯分布：${\hat{X}}_{k-1}\sim N({\hat{x}}_{k-1},P_{k-1})$
根据预测方程：$x_k=A_k{X}_{k-1}+B_k{u}_k+w_k$
通过上面的高斯分布定理可以推导出k时刻的预测状态，服从高斯分布：${\hat{X}}_{\bar{k}}\sim N(A_k{\hat{x}}_{k-1}+B_k{u}_k,A_k{P}_{k-1}{A}_k^T+Q)$
从而得到k时刻的预测状态（先验状态向量和对应的估计协方差）：
$$\begin{gathered} {\hat{x}}_{\bar{k}}=A_k{\hat{x}}_{k-1}+B_k{u}_k \\ {P}_{\bar{k}}=A_k{P}_{k-1}{A}_k^T+Q \end{gathered}$$
注意：

• 因为k时刻的状态向量时由k-1时刻的估计状态向量获取的，因此此时的${\hat{x}}_{\bar{k}}$表示的是k时刻的先验估计状态；
• 由于这里所有能的参数都是矩阵形式，因此对于k时刻对应的状态向量协方差矩阵对应的转换如上2式所示。

更新

定理2：
两个满足高斯分布的量乘积以后的状态依旧满足高斯分布。
已知X服从高斯分布：$X\sim N({\mu }_1,{\sigma }_1^2)$, Y服从高斯分布：$Y\sim N({\mu }_2,{\sigma }_2^2)$，则XY服从高斯分布：$XY\sim N(\mu ,{\sigma }^2)$
$$\begin{gathered} K=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2} \\ \mu ={\mu }_1+K({\mu }_2-{\mu }_1) \\ {\sigma }^2=(1-K){\sigma }_1^2 \end{gathered}$$

由上面知道k时刻的预测状态服从高斯分布，${\hat{X}}_{\bar{k}}\sim N({\hat{x}}_{\bar{k}},P_{\bar{k}})$
且由测量方程：$z_k=H_k{x}_k+v_k$
按照定理1进行转换，即将k时刻的预测状态转换到k时刻的测量状态，同样服从高斯分布：${\hat{Z}}_{\bar{k}}\sim N(H_k{\hat{x}}_{\bar{k}},H_k{P}_{\bar{k}}{H}^{T}k)$

注意：
这里协方差矩阵$H_k{P}_{\bar{k}}{H}_k^T$没有$+R$是因为这里只是状态的转换，并不包含实际的测量。测量本身服从高斯分布：$Z_k\sim N(z_k,R)$
我们对上面两个高斯分布进行相乘，即k时刻预测的乘以k时刻测量的，便得到k时刻状态的最优估计：${\hat{X}}_k\sim N({\hat{x}}_k,P_k)$
这里需要注意的是，我们同样需要把k时刻状态的最优估计转换到测量状态下：${\hat{Z}}_k\sim N(H_k{\hat{x}}_k,H_k{P}_k{H}_k^T)$。
按照定理2：${\hat{Z}}_{\bar{k}}\times Z_k={\hat{Z}}_k$
$$\begin{gathered} K^{\prime }=\frac{H_k{P}_{\bar{k}}{H}_k^T}{H_k{P}_{\bar{k}}{H}_k^T+R} \\ {H}_k{\hat{x}}_k=H_k{\hat{x}}_{\bar{k}}+K^{\prime }(z_k-H_k{\hat{x}}_{\bar{k}}) \\ {H}_k{P}_k{H}_k^T=(I-K^{\prime })H_k{P}_{\bar{k}}{H}_k^T \end{gathered}$$
其中：$K_k=\frac{K^{\prime }}{H_k}=\frac{P_{\bar{k}}{H}_k^T}{H_k{P}_{\bar{k}}{H}_k^T+R}$
将上式中的K替换后，即可得到更新步骤中的三个核心公式：
$$\begin{gathered} K_k=\frac{P_{\bar{k}}{H}_k^T}{H_k{P}_{\bar{k}}{H}_k^T+R} \\ {\hat{x}}_k={\hat{x}}_{\bar{k}}+K_k(z_k-H_k{\hat{x}}_{\bar{k}}) \\ {P}_k=(I-K_k{H}_k)P_{\bar{k}} \end{gathered}$$
至此，我们得到了卡尔曼滤波的最终结果：${\hat{x}}_k$和$P_k$
注意：
这里需要注意的一点是要将状态方程全部转换到测量状态下才可以进行高斯相乘。否则的话，他们的矩阵都不匹配。

参数形式

上面的公式中的变量及参数都是矩阵的形式，并且矩阵的行列并不一致。这里举例说明下。
假设状态向量$x$为$x_{n\times 1}$，测量向量$z$为$z_{m\times 1}$，控制变量$\mu$为${\mu }_{a\times 1}$，则所有相关参数的对应的维度如下：

$$\{\begin{array}{r}{A}_{n\times n}\\ B_{n\times a}\\ P_{n\times n}\\ Q_{n\times n}\\ H_{m\times n}\\ K_{n\times m}\\ R_{m\times m}\\ I_{n\times n}\end{array}$$

误差状态扩展卡尔曼滤波器(Error-State Extended Kalman Filter(ES-EKF))

载体状态可以分为两部分：
$$x=\hat{x}+\delta x$$
其中$x$表示当前状态真值，是我们试图获取的结果，$\hat{x}$表示Nominal State，即获取的估计状态，占据主要部分，$\delta x$表示Error State，即真值与估计状态进行的差值，占据很小一部分。
算法流程：
预测：
通过已知的上一时刻的nominal state状态对当前时刻的状态量以及进行估计。
$$\begin{gathered} {\hat{x}}_{\bar{k}}=A_k{\hat{x}}_{k-1}+B_k{u}_k \\ {P}_{\bar{k}}=A_k{P}_{k-1}{A}_k^T+Q \end{gathered}$$
获取卡尔曼增益：
$$K_k=\frac{P_{\bar{k}}{H}_k^T}{H_k{P}_{\bar{k}}{H}_k^T+R}$$
获取对应误差状态：
$$\delta {\hat{x}}_k=K_k(z_k-H_k{\hat{x}}_{\bar{k}})$$
更新nominal状态向量：
$${\hat{x}}_k={\hat{x}}_{\bar{k}}+\delta {\hat{x}}_k$$
更新协方差矩阵：
$$P_k=(I-K_k{H}_k)P_{\bar{k}}$$

ES-EKF优势：

1. 与nominal state 相比，error state 具有更好的线性。
2. 对于3D空间的旋转同样可以运行。
3. es-ekf将状态分为大的nominal state和小的error state。
4. es-ekf使用局部线性化估计error state并用它来纠正nominal state。
5. es-ekf可以解决3D空间的旋转问题。

参考：
简明ESKF推导

FAST-LIO 中卡尔曼理论解析

在FAST-LIO中涉及到的ES-EKF方法分别位于IMU去畸变以及整体位姿估计部分：

IMU去畸变

涉及的参数解释

IMU所有状态向量共24维：
pos：位置
rot：欧拉角
offset_R_L_I：IMU和激光雷达之间的安装角（旋转矩阵）
offset_T_L_I：IMU和激光雷达之间的杆臂 （平移向量）
vel：速度
bg：陀螺仪零偏
ba：加速度计零偏
grav：重力向量

/*
 * 状态量
 */
MTK_BUILD_MANIFOLD(state_ikfom,
((vect3, pos))//空间位置
((SO3, rot))//旋转向量
((SO3, offset_R_L_I))//偏移旋转量
((vect3, offset_T_L_I))//偏移平移量
((vect3, vel))//速度
((vect3, bg))//角速度偏移
((vect3, ba))//加速度偏移
((S2, grav))//重力
);

IMU输出数据为三轴角速度和三轴线加速度，共6维：
acc：三轴加速度；
gyro：三轴角速度；

/*
 * 测量值，IMU输入
 */
MTK_BUILD_MANIFOLD(input_ikfom,
((vect3, acc))
((vect3, gyro))
);

IMU噪声参数，共12维：
ng：旋转随机噪声；
na：加速度随机噪声；
nbg：角度偏执随机噪声；
nba：加速度偏执随机噪声；

MTK_BUILD_MANIFOLD(process_noise_ikfom,
((vect3, ng))
((vect3, na))
((vect3, nbg))
((vect3, nba))
);

IMU去畸变实现过程

该过程主要位于void UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_in_out);函数中，该函数将相邻两点云之间的IMU帧对应的空间位姿进行保存，并以此为基础实现对该时刻所有点云位置的去畸变处理；

/*
 * IMU数据畸变处理
 */
void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
{
    /*** add the imu of the last frame-tail to the of current frame-head ***/
    auto v_imu = meas.imu;//拿到当前的imu数据
    v_imu.push_front(last_imu_); //将上一帧最后尾部的imu添加到当前帧头部的imu
    const double &imu_beg_time = v_imu.front()->header.stamp.toSec();//拿到当前帧头部的imu的时间（也就是上一帧尾部的imu时间戳）
    const double &imu_end_time = v_imu.back()->header.stamp.toSec(); //拿到当前帧尾部的imu的时间
    // pcl开始和结束的时间戳
    const double &pcl_beg_time = meas.lidar_beg_time;
    const double &pcl_end_time = meas.lidar_end_time;

    /*** sort point clouds by offset time ***/
    // 根据点云中每个点的时间戳对点云进行重排序
    pcl_out = *(meas.lidar);
    sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
    // cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
  //          <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;

    /*** Initialize IMU pose ***/
    // 获取上一次KF估计的后验状态作为本次IMU预测的初始状态
    state_ikfom imu_state = kf_state.get_x();
    //清空IMUpose
    IMUpose.clear();
    //将初始状态加入IMUpose中,包含有时间间隔，上一帧加速度，上一帧角速度，上一帧速度，上一帧位置，上一帧旋转矩阵
    IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));

    /*** forward propagation at each imu point ***/
    //前向传播对应的参数
    V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;// angvel_avr为平均角速度，acc_avr为平均加速度，acc_imu为imu加速度，vel_imu为imu速度，pos_imu为imu位置
    M3D R_imu;// imu旋转矩阵

    double dt = 0;//时间间隔

    input_ikfom in;// eksf 传入的参数
    // 遍历本次估计的所有IMU测量并且进行积分，离散中值法 前向传播
    // 将所有IMU的状态实现进行了滤波计算，保留了所有IMU帧对应的状态
    for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
    {
        auto &&head = *(it_imu); //拿到当前帧的imu数据
        auto &&tail = *(it_imu + 1); //拿到下一帧的imu数据
        //判断时间先后顺序 不符合直接continue
        if (tail->header.stamp.toSec() < last_lidar_end_time_)    continue;
        // 中值积分，选取两帧IMU之间的角速度和加速度均值
        angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
                0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
                0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
        acc_avr   <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
                0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
                0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);

        // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
        //通过重力数值对加速度进行一下微调
        acc_avr     = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;
        //如果IMU开始时刻早于上次雷达最晚时刻(因为将上次最后一个IMU插入到此次开头了，所以会出现一次这种情况)
        if(head->header.stamp.toSec() < last_lidar_end_time_)
        {
            //从上次雷达时刻末尾开始传播 计算与此次IMU结尾之间的时间差
            dt = tail->header.stamp.toSec() - last_lidar_end_time_;
            // dt = tail->header.stamp.toSec() - pcl_beg_time;
        }
        else
        {//两个IMU时刻之间的时间间隔
            dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
        }
        // 原始测量的中值作为更新
        in.acc = acc_avr;
        in.gyro = angvel_avr;
        // 配置协方差矩阵
        Q.block<3, 3>(0, 0).diagonal() = cov_gyr;
        Q.block<3, 3>(3, 3).diagonal() = cov_acc;
        Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
        Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
        // IMU前向传播，每次传播的时间间隔为dt
        kf_state.predict(dt, Q, in);

        /* save the poses at each IMU measurements */
        // 保存IMU预测过程的状态
        imu_state = kf_state.get_x();
        //计算出来的角速度与预测的角速度的差值
        angvel_last = angvel_avr - imu_state.bg;
        //计算出来的加速度与预测的加速度的差值,并转到IMU坐标系下
        acc_s_last  = imu_state.rot * (acc_avr - imu_state.ba);
        for(int i=0; i<3; i++)
        { //加上重力得到世界坐标系的加速度
            acc_s_last[i] += imu_state.grav[i];
        }
        //后一个IMU时刻距离此次雷达开始的时间间隔
        double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;
        //保存IMU预测过程的状态
        IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
    }

    /*** calculated the pos and attitude prediction at the frame-end ***/
    // 把最后一帧IMU测量也补上
    // 判断雷达结束时间是否晚于IMU，最后一个IMU时刻可能早于雷达末尾 也可能晚于雷达末尾
    double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
    dt = note * (pcl_end_time - imu_end_time);
    kf_state.predict(dt, Q, in);

    imu_state = kf_state.get_x();
    last_imu_ = meas.imu.back();
    last_lidar_end_time_ = pcl_end_time;

    /*** undistort each lidar point (backward propagation) ***/
    /*** 在处理完所有的IMU预测后，剩下的就是对激光的去畸变了 ***/
    // 基于IMU预测对lidar点云去畸变
    if (pcl_out.points.begin() == pcl_out.points.end()) return;

    auto it_pcl = pcl_out.points.end() - 1;
    for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
    {
        auto head = it_kp - 1;
        auto tail = it_kp;
        R_imu<<MAT_FROM_ARRAY(head->rot); //拿到前一帧的IMU旋转矩阵
        // cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
        vel_imu<<VEC_FROM_ARRAY(head->vel); //拿到前一帧的IMU速度
        pos_imu<<VEC_FROM_ARRAY(head->pos); //拿到前一帧的IMU位置
        acc_imu<<VEC_FROM_ARRAY(tail->acc);//拿到后一帧的IMU加速度
        angvel_avr<<VEC_FROM_ARRAY(tail->gyr);//拿到后一帧的IMU角速度
        //点云时间需要迟于前一个IMU时刻 因为是在两个IMU时刻之间去畸变，此时默认雷达的时间戳在后一个IMU时刻之前
        for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)//时间除以1000单位化为s
        {
            dt = it_pcl->curvature / double(1000) - head->offset_time;//点到IMU开始时刻的时间间隔
            /*变换到“结束”帧，仅使用旋转
               *注意：补偿方向与帧的移动方向相反
               *所以如果我们想补偿时间戳i到帧e的一个点
               * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei)  其中T_ei在全局框架中表示*/
            /* Transform to the 'end' frame, using only the rotation
             * Note: Compensation direction is INVERSE of Frame's moving direction
             * So if we want to compensate a point at timestamp-i to the frame-e
             * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
            M3D R_i(R_imu * Exp(angvel_avr, dt));//点所在时刻的旋转

            V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);//点所在时刻的位置(雷达坐标系下)
            V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
            //从点所在的世界位置-雷达末尾世界位置
            //.conjugate()取旋转矩阵的转置    (可能作者重新写了这个函数 eigen官方库里这个函数好像没有转置这个操作  实际测试cout矩阵确实输出了转置)
            // imu_state.offset_R_L_I是从雷达到惯性的旋转矩阵 简单记为I^R_L
            // imu_state.offset_T_L_I是惯性系下雷达坐标系原点的位置简单记为I^t_L
            //下面去畸变补偿的公式这里倒推一下
            // e代表end时刻
            // P_compensate是点在末尾时刻在雷达系的坐标 简记为L^P_e
            //将右侧矩阵乘过来并加上右侧平移
            //左边变为I^R_L * L^P_e + I^t_L= I^P_e 也就是end时刻点在IMU系下的坐标
            //右边剩下imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei
            // imu_state.rot.conjugate()是结束时刻IMU到世界坐标系的旋转矩阵的转置 也就是(W^R_i_e)^T
            // T_ei展开是pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos也就是点所在时刻IMU在世界坐标系下的位置 - end时刻IMU在世界坐标系下的位置 W^t_I-W^t_I_e
            //现在等式两边变为 I^P_e =  (W^R_i_e)^T * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + W^t_I - W^t_I_e
            //(W^R_i_e) * I^P_e + W^t_I_e = (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + W^t_I
            // 世界坐标系也无所谓时刻了 因为只有一个世界坐标系 两边变为
            // W^P = R_i * I^P+ W^t_I
            // W^P = W^P
            V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!

            // save Undistorted points and their rotation
            //todo 暂时屏蔽IMU去畸变功能
            it_pcl->x = P_compensate(0);
            it_pcl->y = P_compensate(1);
            it_pcl->z = P_compensate(2);

            if (it_pcl == pcl_out.points.begin()) break;
        }
    }
}

该方法对应论文中的阐述可以分为前向传导和反相传导俩部分，首先对于前向传导，其主要任务是将两点云帧之间的IMU位姿进行估计保存，用于后续的去畸变处理工作。
前向传导
void predict(double &dt, processnoisecovariance &Q, const input &i_in)函数用于估计下一帧IMU的状态，其中i_in中涉及到的输入信息为三轴角速度和三轴线加速度，源码中以中值积分的形式实现的。

// 中值积分，选取两帧IMU之间的角速度和加速度均值
        angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
                0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
                0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
        acc_avr   <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
                0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
                0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);
// IMU前向传播，每次传播的时间间隔为dt
        kf_state.predict(dt, Q, in);

而在predict函数中，利用flatted_state f_ = f(x_, i_in);// m*1对其中的位置、旋转角、加速度导数求解并一匀速模型对新的IMU状态进行估计。随后根据上述以及论文中涉及到的ES-EKF对应的状态矩阵和噪声矩阵模型$F_x$、$F_w$进行填充并更新协方差$P$矩阵。

F_x1 += f_x_final * dt;
P_ = (F_x1) * P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose();

后向传导
对于后向传导，这是利用前向过程中获取的IMU位姿信息对点云信息去畸变处理。

位姿估计中的EKF解释