自动驾驶与机器人中的slam技术（高翔）第三章（卡尔曼滤波）run_eskf_gins.cc代码笔记

# W为世界坐标，G为GNSS坐标，B为本体坐标，以IMU位置为准。
# 状态量 x = [p,v,R，bg，ba，g].transpose
# 误差量 dx_ = [p,v,Log(R)，bg，ba，g].transpose

1、读取odom的轮速，判断是否静止
2、如果imu未初始化且静止了，记录静止后第一次imu出现的时间t1。记录从t1开始持续t2时间的imu数据进行初始化
3、使用imu静止时的平均加速度作为重力加速度，imu再加上重力，因为imu测量到的是反向重力
4、使用各个方向计算的方差作为测量噪声，均值作为零偏，使用其中一个值作为eskf的观测噪声的协方差（这意味着观测噪声可以由真实数据的协方差来表示），
    并初始化eskf参数,dx_ = 0,cov_ = Mat18T::Identity();Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;
    gnss_pos_noise_ = 0.1;// GNSS位置噪声。gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS旋转噪声
5、等待GNSS数据，current_t为最近一次时间
6、将GNSS转为utm坐标，使用RTK天线的外参求出到B坐标时的平移TWB
7、使用GNSS得到的R、p，和IMU初始化时的bg、ba，创建整体名义状态变量类型（t, R_, p_, v_, bg_, ba_）用来更新ui，v_是初始值
8、收到imu数据更新x：
    p_ = new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;
    v_ = new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;
    R_ = new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);
9、求取线性化后的转移矩阵F = [I I*dt            0             0       0        0         （状态量：
                          0  I    -R*(acce_ - ba_)^*dt    0     -R_*dt   I*dt
                          0  0     EXP(-(gyro_-bg_)*dt) -I*dt     0        0
                          0 0 0 0 I 0
                          0 0 0 0 0 I]
10、预测dx_ = F * dx_;估计协方差cov_ = F * cov_.eval() * F.transpose() + Q_;
11、如果持续受到IMU，则继续执行8、9、10
12、直到收到GNSS，更新
    （1）观测矩阵，GNSS得到的是R和p,因此 H = [ I 0 0 0 0 0 
                                          0 0 I 0 0 0]
13、卡尔曼增益 K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse(); 
    //V为观测噪声，V = diagonal() << gnss_pos_noise_，gnss_pos_noise_，gnss_pos_noise_，gnss_ang_noise_，gnss_ang_noise_，gnss_ang_noise_

14、求取GNSS观测值与IMU预测值的差，innov = z-h(x) = [pgnss - ppred, Log(R_T * Rgnss)]T 
15、更新：误差量dx_ = K * innov;
    cov_ = (Mat18T::Identity() - K * H) * cov_;
16、名义量+误差量:
        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));
        bg_ += dx_.template block<3, 1>(9, 0);
        ba_ += dx_.template block<3, 1>(12, 0);
        g_ += dx_.template block<3, 1>(15, 0);
17、对误差更新量置零，旋转量导致估计矩阵需要切空间投影：
    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(); 
    dx_ = 0;
18、结束一轮操作，如果继续接收到IMU，则是在名义状态量上不停积分，如果是gnss，则修正添加误差量。