武汉大学KF-GINS开源代码学习3--newImuProcess()

这次进行newImuProcess的学习，这个函数是实现松组合的功能，松组合需要INS和GNSS的数据进行EKF滤波融合，首先需要判断是否有GNSS信号来临，再判断INS和GNSS的数据是否表示同一时刻的，即两者之间的关系，之后再进行相应的融合操作。

其中有部分内容参考博客：https://blog.youkuaiyun.com/daoge2666/article/details/133376618

目录

GNSS信号是否有效

确定IMU和GNSS的时刻

根据时刻确定融合策略

不在两个IMU时间

靠近前一个时刻IMU

 靠近当前时刻IMU

GNSS在IMU两者之间，但不靠近任何一个

 协方差检测和更新IMU与PVA

---

GNSS信号是否有效

如果GNSS有效，则将量测更新时间设置为 GNSS 时间：

    double updatetime = gnssdata_.isvalid ? gnssdata_.time : -1;

确定IMU和GNSS的时刻

    int res = isToUpdate(imupre_.time, imucur_.time, updatetime);

INS和GNSS的数据会存在以下四种情况，如图所示：

int GIEngine::isToUpdate(double imutime1, double imutime2, double updatetime) const {

    if (abs(imutime1 - updatetime) < TIME_ALIGN_ERR) {
        // 更新时间靠近imutime1
        // updatetime is near to imutime1
        return 1;
    } else if (abs(imutime2 - updatetime) <= TIME_ALIGN_ERR) {
        // 更新时间靠近imutime2
        // updatetime is near to imutime2
        return 2;
    } else if (imutime1 < updatetime && updatetime < imutime2) {
        // 更新时间在imutime1和imutime2之间, 但不靠近任何一个
        // updatetime is between imutime1 and imutime2, but not near to either
        return 3;
    } else {
        // 更新时间不在imutimt1和imutime2之间，且不靠近任何一个
        // updatetime is not bewteen imutime1 and imutime2, and not near to either.
        return 0;
    }
}

 IMU和GNSS时间小于0.001则默认为时间对齐了，IMU的采样时间设置为0.005s，说明更新时间对齐误差要小于IMU的一个采样时间。

    const double TIME_ALIGN_ERR = 0.001;

根据时刻确定融合策略

判断GNSS的时刻与前一时刻和当前时刻IMU的关系：

不在两个IMU时间

如果GNSS的时间不在两个IMU时间之间（res==0），说明此时没有GNSS信号，则不用进行量测更新，只进行预测更新即可。

    if (res == 0) {
        // 只传播导航状态
        // only propagate navigation state
        insPropagation(imupre_, imucur_);

靠近前一个时刻IMU

如果GNSS的时间靠近前一个时刻IMU(res==1)，则由于前一时刻的IMU已经是预测更新之后的值，不需要再进行更新，因此只需要将前一时刻的IMU数据和GNSS数据进行融合（gnssUpdate），将融合得到的结果修正状态变量（stateFeedback）即可；

修正完成后即得到前一时刻的最优估计，再和当前时刻的IMU数据进行预测更新。

代码如下所示：

    } else if (res == 1) {
        // GNSS数据靠近上一历元，先对上一历元进行GNSS更新
        // gnssdata is near to the previous imudata, we should firstly do gnss update
        gnssUpdate(gnssdata_);
        stateFeedback();

        pvapre_ = pvacur_;
        insPropagation(imupre_, imucur_);

注意：在量测更新之后得到的pvacur_，而原本没有GNNS数据来临，进行预测更新时，使用的是pvapre_，此时中间进行了一步前一时刻的量测更新，因此到下一步预测更新时，要将pvacur_赋值给pvapre_，即相当于是更新了pvapre_，为了下一步中当前时刻的预测更新。

 靠近当前时刻IMU

如果GNSS数据是靠近当前时刻IMU(res==2)，则融合策略如下所示：

(1)先利用前一时刻和当前时刻的IMU数据进行预测更新，得到当前时刻的值(insPropagation);

(2)再进行量测更新(gnssUpdate);

(3)更新完成后进行状态反馈修正(stateFeedback)。

代码如下所示：

    } else if (res == 2) {
        // GNSS数据靠近当前历元，先对当前IMU进行状态传播
        // gnssdata is near current imudata, we should firstly propagate navigation state
        insPropagation(imupre_, imucur_);
        gnssUpdate(gnssdata_);
        stateFeedback();

GNSS在IMU两者之间，但不靠近任何一个

如果GNSS数据在两个IMU数据之间(不靠近任何一个)，则融合策略如下所示：

(1)先调用 imuInterpolate() 根据两帧 IMU 量测插值到 GNSS 时间戳，得到 GNSS 时刻 IMU 量测值 midimu，实现的原理图如下所示。

(2)然后利用前一时刻和中间时刻的IMU数据进行预测更新(insPropagation)；

(3)预测更新完成后和GNSS的数据一起进行量测更新(gnssUpdate)；

(4)量测更新完成后修正状态变量(stateFeedback)，此时的状态变量是中间时刻的，需要进一步递推到当前时刻(t2)；

(5)更新pva，然后利用中间时刻和当前时刻的IMU数据进行预测更新(insPropagation)。

代码如下所示：

    } else {
        // GNSS数据在两个IMU数据之间(不靠近任何一个), 将当前IMU内插到整秒时刻
        // gnssdata is between the two imudata, we interpolate current imudata to gnss time
        IMU midimu;
        imuInterpolate(imupre_, imucur_, updatetime, midimu);

        // 对前一半IMU进行状态传播
        // propagate navigation state for the first half imudata
        insPropagation(imupre_, midimu);

        // 整秒时刻进行GNSS更新，并反馈系统状态
        // do GNSS position update at the whole second and feedback system states
        gnssUpdate(gnssdata_);
        stateFeedback();

        // 对后一半IMU进行状态传播
        // propagate navigation state for the second half imudata
        pvapre_ = pvacur_;
        insPropagation(midimu, imucur_);

IMU插值(imuInterpolate)的原理如下所示：

时间轴
|----------------|----------------|----------------|
t₁ (imu1)       t_mid           t₂ (原imu2)
                 ↑               ↑
               midimu        调整后的imu2

数据分割示意：
原imu2.dtheta = Δθ_total
midimu.dtheta = Δθ_total * λ (λ = (t_mid - t₁)/(t₂ - t₁))
调整后imu2.dtheta = Δθ_total - midimu.dtheta = Δθ_total * (1 - λ)

时间间隔分割：
原Δt = t₂ - t₁
midimu.dt = t_mid - t₁ = λΔt
调整后imu2.dt = t₂ - t_mid = (1 - λ)Δt

注意：原IMU2的数据表示t1~t2时刻的增量数据，现在t1变成了t_mid，因此imu2的数据也需要更改，更改为t_mid~t2时刻的增量数据，即用原来的IMU2增量数据减去t_mid时刻的增量数据即可。

 协方差检测和更新IMU与PVA

(1)检查协方差的每一个对角线元素是否小于0；

(2)更新IMU和PVA的值；

    checkCov();

    // 更新上一时刻的状态和IMU数据
    // update system state and imudata at the previous epoch
    pvapre_ = pvacur_;
    imupre_ = imucur_;