MSCKF5讲：后端代码分析

文章目录

MSCKF5讲：后端代码分析
1 初始化initialize()
- 1.1 加载参数
- 1.2 初始化`IMU`连续噪声协方差矩阵
- 1.3 卡方检验
- 1.4 接收与订阅话题createRosIO()
2 IMU静止初始化
3 重置resetCallback()
4 featureCallback
- 4.1 IMU初始化判断
- 4.2 IMU积分
- - 4.2.1 batchImuProcessing
  - 4.2.2 processModel
  - - ① 移除imu测量偏置
    - ② 计算系数矩阵F和噪声矩阵G（连续，误差状态）
    - ③ 计算状态转移矩阵Phi（离散化的F，误差状态）
    - ④ IMU状态预测
    - - <4.1> `Omega`矩阵
      - <4.2> 获取k时刻的imu状态
      - <4.3> 预测姿态 $_G^Iq^T$
      - <4.4> 预测速度v和位置p
    - ⑤ 可观性约束
    - ⑥ 更新0空间约束后的状态转移矩阵phi和误差状态协方差矩阵
    - ⑦ 更新imu状态量与相机状态量交叉的部分
    - ⑧ 强制对称协方差矩阵
    - ⑨ 更新零空间
- 4.3 状态增广 stateAugmentation
- - 4.3.1 利用最新的imu状态计算cam状态
  - 4.3.2 记录相机状态- `imu_state.id`
  - 4.3.3 更新协方差矩阵
  - - ① 计算雅可比矩阵
    - ② 计算增广协方差矩阵
- 4.4 添加特征点观测
- 4.5 利用视觉观测更新状态
- - 4.5.1 特征点管理
  - 4.5.2 计算误差量雅可比与重投影误差
  - - ① featureJacobian()
    - ② measurementJacobian()
    - - <1> 取出观测z和位姿Rwc、路标点p_w
      - <2> 计算雅可比H
      - <3> 对H矩阵的可观测性约束
      - <4> 计算残差r
  - ③ 左零空间投影
  - 4.5.3 状态更新
  - - ① 降维
    - ② 卡尔曼滤波更新
- 4.6 pruneCamStateBuffer
- 4.7 发布位姿publish
- 4.8 是否重置系统onlineReset

这里程序实际上是两个进程

msckf_vio_nodelet.h

namespace msckf_vio {
class MsckfVioNodelet : public nodelet::Nodelet {
public:
  MsckfVioNodelet() { return; }
  ~MsckfVioNodelet() { return; }

private:
  virtual void onInit();	// 虚函数
  MsckfVioPtr msckf_vio_ptr;		// boost::shared_ptr<MsckfVio>类指针
};
} // end namespace msckf_vio

nodelet/nodelet.h：
- 提供了 Nodelet 类的定义，是用于实现节点的基类。
- Nodelet 是一种轻量级的ROS节点，它可以在同一个进程中共享节点管理和ROS通信，以提高系统的效率。
- Nodelet 允许将不同的ROS节点合并到一个进程中，从而减少通信开销。
pluginlib/class_list_macros.h：
- 提供了一组宏，用于导出和加载ROS插件，包括节点类（nodelets）。
- PLUGINLIB_EXPORT_CLASS 是该库中的一个重要宏，用于将一个类导出为ROS插件，以便它可以在运行时被动态加载。
- 通过这些宏，ROS能够在运行时根据类名查找和加载相应的插件。

msckf_vio_nodelet.cpp

在这里，getPrivateNodeHandle() 用于获取一个私有节点句柄，然后将这个句柄传递给 MsckfVio 类的构造函数。这样做的目的可能是为了让 MsckfVio 类能够在私有命名空间下访问参数，主题等ROS资源。

在ROS中，私有节点句柄的名称通常是在节点命名空间后添加 “~” 符号来表示的。例如，如果节点的名称是 /my_node，那么私有节点句柄的名称将是 /my_node/。这有助于确保节点的参数和主题不会与其他节点的冲突。

#include <msckf_vio/msckf_vio_nodelet.h>

namespace msckf_vio {
void MsckfVioNodelet::onInit() {
  // 通过 reset 方法初始化为一个新的 MsckfVio 对象
  // 该对象使用 getPrivateNodeHandle() 进行初始化
  msckf_vio_ptr.reset(new MsckfVio(getPrivateNodeHandle()));
  if (!msckf_vio_ptr->initialize()) {
    ROS_ERROR("Cannot initialize MSCKF VIO...");
    return;
  }
  return;
}

// MsckfVioNodelet 类被导出，以便 ROS 可以动态加载它
PLUGINLIB_EXPORT_CLASS(msckf_vio::MsckfVioNodelet,
    nodelet::Nodelet);

}

struct StateServer {
    IMUState imu_state;
    Cam cam_states;

    // State covariance matrix	
    Eigen::MatrixXd state_cov;				// 误差状态(包括IMU和Cam)协方差矩阵
    Eigen::Matrix<double, 12, 12> continuous_noise_cov;		// IMU连续噪声协方差
};

1 初始化initialize()

我们要搞清楚，在帧Frame的构造函数中，做了那些工作，与哪些函数相关联。

后端初始化函数	内容
1 加载参数	`loadParameters()`
2 初始化`IMU`噪声协方差矩阵	初始化`state_server.continuous_noise_cov`中 $n_{g},n_{a},n_{bg},n_{ba}$
3 卡方检验	`chi_squared_test_table`
4 订阅与发布`ROS`话题	调用`createRosIO()`

1.1 加载参数

bool MsckfVio::initialize() 
{
  // 1. 加载参数
  if (!loadParameters()) return false;			
  ...
  return true;
}


bool MsckfVio::loadParameters() {
  // 世界坐标系、里程计坐标系
  nh.param<string>("fixed_frame_id", fixed_frame_id, "world");
  nh.param<string>("child_frame_id", child_frame_id, "robot");

  // 发布坐标
  nh.param<bool>("publish_tf", publish_tf, true);
  
  // 帧率
  nh.param<double>("frame_rate", frame_rate, 40.0);
  
  // 没有”跟丢“概念	判断是否发散  	状态的协方差应该在一个范围内，超过阈值即resize
  nh.param<double>("position_std_threshold", position_std_threshold, 8.0);

  // 判断是否删除状态
  nh.param<double>("rotation_threshold", rotation_threshold, 0.2618);
  nh.param<double>("translation_threshold", translation_threshold, 0.4);
  nh.param<double>("tracking_rate_threshold", tracking_rate_threshold, 0.5);

  // Feature optimization parameters  
  nh.param<double>("feature/config/translation_threshold",
      Feature::optimization_config.translation_threshold, 0.2);

  // IMU参数(标准差)  预测			默认值是一些经验值，便宜的IMU这方面不好说影响
  nh.param<double>("noise/gyro", IMUState::gyro_noise, 0.001);				// ng
  nh.param<double>("noise/acc", IMUState::acc_noise, 0.01);					// na
  nh.param<double>("noise/gyro_bias", IMUState::gyro_bias_noise, 0.001);	// nbg
  nh.param<double>("noise/acc_bias", IMUState::acc_bias_noise, 0.01);		// nba
  
  // 观测---特征噪声---经验值---
  nh.param<double>("noise/feature", Feature::observation_noise, 0.01);

  // 标定得到是标准差，协方差矩阵应该是方差，使用方差代替上面标准差
  IMUState::gyro_noise *= IMUState::gyro_noise;
  IMUState::acc_noise *= IMUState::acc_noise;
  IMUState::gyro_bias_noise *= IMUState::gyro_bias_noise;
  IMUState::acc_bias_noise *= IMUState::acc_bias_noise;
  Feature::observation_noise *= Feature::observation_noise;

   /*设置IMU状态*/
  // 对于旋转R和偏移t，初始化大都默认设置为0.但是对于初始的速度v和偏差b，设置为0是否合理？
  // 设置v = 0
  nh.param<double>("initial_state/velocity/x",state_server.imu_state.velocity(0), 0.0);
  nh.param<double>("initial_state/velocity/y",state_server.imu_state.velocity(1), 0.0);
  nh.param<double>("initial_state/velocity/z",state_server.imu_state.velocity(2), 0.0);

  // The initial covariance of orientation and position can be set to 0. 
  // But for velocity, bias and extrinsic parameters,
  // there should be nontrivial uncertainty.
  // 初始化误差状态的协方差。 (这里和上面标准差不同之处在于，上面是IMU模型，这里是指状态量！)
    
  // 误差状态量:速度 、 g 、加速度
  double gyro_bias_cov, acc_bias_cov, velocity_cov;
  nh.param<double>("initial_covariance/velocity",velocity_cov, 0.25);
  nh.param<double>("initial_covariance/gyro_bias",gyro_bias_cov, 1e-4);
  nh.param<double>("initial_covariance/acc_bias",acc_bias_cov, 1e-2);

  // 旋转、平移
  double extrinsic_rotation_cov, extrinsic_translation_cov;
  nh.param<double>("initial_covariance/extrinsic_rotation_cov",extrinsic_rotation_cov, 3.0462e-4);
  nh.param<double>("initial_covariance/extrinsic_translation_cov",extrinsic_translation_cov, 1e-4);

  
  // 对应论文,不过那个讲师说后面那个外参协方差在其他里面优化效果不好。为什么旋转平移就可以是0？因为在正式开始之前我们通过初始化找好了重力方向，确定了第一帧的位姿
  // 0~3 角度
  // 3~6 陀螺仪偏置
  // 6~9 速度 
  // 9~12 加速度计偏置
  // 12~15 位移
  // 15~18 外参旋转
  // 18~21 外参平移
  state_server.state_cov = MatrixXd::Zero(21, 21);
  for (int i = 3; i < 6; ++i)
    state_server.state_cov(i, i) = gyro_bias_cov;
  for (int i = 6; i < 9; ++i)
    state_server.state_cov(i, i) = velocity_cov;
  for (int i = 9; i < 12; ++i)
    state_server.state_cov(i, i) = acc_bias_cov;
  for (int i = 15; i < 18; ++i)
    state_server.state_cov(i, i) = extrinsic_rotation_cov;
  for (int i = 18; i < 21; ++i)
    state_server.state_cov(i, i) = extrinsic_translation_cov;

  // Transformation offsets between the frames involved.
  Isometry3d T_imu_cam0 = utils::getTransformEigen(nh, "cam0/T_cam_imu");
  Isometry3d T_cam0_imu = T_imu_cam0.inverse();

  state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();
  state_server.imu_state.t_cam0_imu = T_cam0_imu.translation();
  CAMState::T_cam0_cam1 = utils::getTransformEigen(nh, "cam1/T_cn_cnm1");
  IMUState::T_imu_body = utils::getTransformEigen(nh, "T_imu_body").inverse();

  // Maximum number of camera states to be stored
  nh.param<int>("max_cam_state_size", max_cam_state_size, 30);
  return true;
}

1.2 初始化`IMU`连续噪声协方差矩阵

为什么都是三维呢？因为角速度和角速度都是三个轴xyz，所以就是3*3的协方差矩阵，为什么初始只有对角线有值(自身方差)，即认为一开始的时候，任意两个轴之间应该是没有关系的。协方差越小，联系越小。

实际上应该可以表示为3*12的矩阵，但是估计写成方阵比较好计算吧。

$n_{g},n_{a},n_{bg},n_{ba}$

state_server.continuous_noise_cov = Matrix<double, 12, 12>::Zero();
state_server.continuous_noise_cov.block<3, 3>(0, 0) = Matrix3d::Identity() * IMUState::gyro_noise;
state_server.continuous_noise_cov.block<3, 3>(3, 3) = Matrix3d::Identity() * IMUState::gyro_bias_noise;
state_server.continuous_noise_cov.block<3, 3>(6, 6) = Matrix3d::Identity() * IMUState::acc_noise;
state_server.continuous_noise_cov.block<3, 3>(9, 9) = Matrix3d::Identity() * IMUState::acc_bias_noise;

1.3 卡方检验

在这里插入图片描述

在统计学中，卡方分布的分位数表示了在给定自由度下，该分布中随机变量落在分位数值以下的概率。具体来说，3.841 是指在 1 自由度的卡方分布中，有 5% 的概率随机变量取值小于或等于 3.841。

这段代码的目的是计算不同自由度下卡方分布的 5% 分位数，并将结果保存在 chi_squared_test_table 中

bool MsckfVio::initialize() 
{

    // 卡方检验表
    // Initialize the chi squared test table with confidence
    // level 0.95.
    for (int i = 1; i < 100; ++i)
    {
        boost::math::chi_squared chi_squared_dist(i);
        chi_squared_test_table[i] = boost::math::quantile(chi_squared_dist, 0.05);
    }

  return true;
}

1.4 接收与订阅话题createRosIO()

bool MsckfVio::initialize() 
{
    if (!createRosIO())
        return false;
}

bool MsckfVio::createRosIO()
{
    // 发送位姿信息与三维点
    odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
    feature_pub = nh.advertise<sensor_msgs::PointCloud2>("feature_point_cloud", 10);

    // 重置     ----服务通信
    reset_srv = nh.advertiseService("reset", &MsckfVio::resetCallback, this);

    // 接收imu数据与前端跟踪的特征
    imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this);
    feature_sub = nh.subscribe("features", 40, &MsckfVio::featureCallback, this);

    // 接受真值，动作捕捉发来的，然后再发送出去，为了再rviz可视化
    mocap_odom_sub = nh.subscribe("mocap_odom", 10, &MsckfVio::mocapOdomCallback, this);
    mocap_odom_pub = nh.advertise<nav_msgs::Odometry>("gt_odom", 1);

    return true;
}

2 IMU静止初始化

imuCallback说白了IMU初始化之后就是不停的订阅IMU数据，放到buffer，直到处理下一帧图像再使用。

void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr &msg)
{   
    // IMU数据没有立即被处理，等待下一帧图像
    // 1. 存放imu数据
    imu_msg_buffer.push_back(*msg);             // std::vector<sensor_msgs::Imu> imu_msg_buffer;

    // 2. 用200个imu数据做静止初始化，不够则不做
    if (!is_gravity_set)
    {
        if (imu_msg_buffer.size() < 200)
            return;
        initializeGravityAndBias();		// 静态初始化
        is_gravity_set = true;
    }
}

initializeGravityAndBias

imu初始化，200个数据必须都是静止时采集的。这里面没有判断是否成功，也就是一开始如果运动会导致轨迹飘。

现在分析一下为什么要做IMU初始化，从IMU噪声模型分析。

对于角速度，静止时，理论时角速度应该为0，但实际不为0，也就是存在偏置 $b_g$ ，所以sum_angular_vel/n求取陀螺仪均值作为偏置；对于加速度，我们得到的时IMU坐标系下的加速度，但实际要的时世界系下的加速度，目的就是为了求解模型中 ${R}_{bw}$ .静止时， $a^w$ 是0，忽略了加速度偏置，既可以得到最简单的计算公式， ${a}^b ={R}_{bw}{g}^w$ 。
$\begin{aligned}\tilde{\boldsymbol{\omega}}^b&=\mathbf{\omega}^b+\mathbf{b}_g+\mathbf{\eta}_g\\\tilde{\mathbf{a}}^b&=\mathbf{R}_{bw}(\mathbf{a}^w{-}\mathbf{g}^w)+\mathbf{b}_a+\mathbf{\eta}_a\end{aligned}$
实际上，如果我们认为g的模长是定值，如9.8，那么也是可以估计加速度偏置的！

在这里插入图片描述

补充：IMU初始化是为了惯性变量获得良好的初始值，这些惯性变量包括重力方向和IMU零偏。先说零偏，MU的零偏不是固定的，是随时间变化的量。由于零偏对MU的影响较大，因此通常作为一个独立的状态来优化。再说重力方向，在视觉惯性模式下，系统以视觉地图初始化成功的第一帧作为世界坐标系原点，此时我们是不知道坐标系中重力的方向的，如果不进行MU初始化，则无法消除重力对IMU积分的影响。IMU初始化的目的就是把图像建立的世界坐标系的之轴拉到和重力方向平行的状态。

/**
 * @brief imu初始化，计算陀螺仪偏置，重力方向以及初始姿态，必须都是静止，且不做加速度计的偏置估计
 */
void MsckfVio::initializeGravityAndBias()
{

    // Initialize gravity and gyro bias.
    
    // 1. 角速度与加速度的和
    Vector3d sum_angular_vel = Vector3d::Zero();
    Vector3d sum_linear_acc = Vector3d::Zero();

    for (const auto &imu_msg : imu_msg_buffer)
    {
        Vector3d angular_vel = Vector3d::Zero();
        Vector3d linear_acc = Vector3d::Zero();
        
		// tf::vectorMsgToEigen 函数将ROS消息对象 imu_msg 中的角速度信息提取并转换为Vector3d 类型
        tf::vectorMsgToEigen(imu_msg.angular_velocity, angular_vel);
        tf::vectorMsgToEigen(imu_msg.linear_acceleration, linear_acc);

        sum_angular_vel += angular_vel;
        sum_linear_acc += linear_acc;
    }

    // 2. 因为假设静止的，因此陀螺仪理论应该都是0，额外读数包括偏置+噪声，但是噪声属于高斯分布
    // 因此这一段相加噪声被认为互相抵消了，所以剩下的均值被认为是陀螺仪的初始偏置
    state_server.imu_state.gyro_bias = sum_angular_vel / imu_msg_buffer.size();

    // 3. 计算重力，忽略加速度计的偏置，那么剩下的就只有重力了
    Vector3d gravity_imu = sum_linear_acc / imu_msg_buffer.size();

    // 初始化重力本来的方向，使估计与惯性系一致，因为一开始测得不一定是(0,0,-g)，所以需要转换
    // 注意，一个向量不会因为坐标系变化而发生变化，所以其模长固定！
    double gravity_norm = gravity_imu.norm();
    IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);	// 得到

    // 求出当前imu状态的重力方向与实际重力方向的旋转---说白了就是IMU坐标系到世界系的旋转Rwi
    Quaterniond q0_i_w = Quaterniond::FromTwoVectors(gravity_imu, -IMUState::gravity);
    // 得出姿态
    state_server.imu_state.orientation = rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());

    return;
}

3 重置resetCallback()

/**
 * @brief 重置
 */
bool MsckfVio::resetCallback(std_srvs::Trigger::Request &req,std_srvs::Trigger::Response &res)
{

    // 暂时不订阅相关的数据
    feature_sub.shutdown();
    imu_sub.shutdown();

    // 重置IMU状态.
    IMUState &imu_state = state_server.imu_state;
    imu_state.time = 0.0;
    imu_state.orientation = Vector4d(0.0, 0.0, 0.0, 1.0);
    imu_state.position = Vector3d::Zero();
    imu_state.velocity = Vector3d::Zero();
    imu_state.gyro_bias = Vector3d::Zero();
    imu_state.acc_bias = Vector3d::Zero();
    imu_state.orientation_null = Vector4d(0.0, 0.0, 0.0, 1.0);
    imu_state.position_null = Vector3d::Zero();
    imu_state.velocity_null = Vector3d::Zero();

    // Remove all existing camera states.
    state_server.cam_states.clear();

    // Reset the state covariance.
    double gyro_bias_cov, acc_bias_cov, velocity_cov;
    nh.param<double>("initial_covariance/velocity", velocity_cov, 0.25);
    nh.param<double>("initial_covariance/gyro_bias", gyro_bias_cov, 1e-4);
    nh.param<double>("initial_covariance/acc_bias", acc_bias_cov, 1e-2);

    double extrinsic_rotation_cov, extrinsic_translation_cov;
    nh.param<double>("initial_covariance/extrinsic_rotation_cov", extrinsic_rotation_cov, 3.0462e-4);
    nh.param<double>("initial_covariance/extrinsic_translation_cov", extrinsic_translation_cov, 1e-4);

    state_server.state_cov = MatrixXd::Zero(21, 21);
    for (int i = 3; i < 6; ++i)
        state_server.state_cov(i, i) = gyro_bias_cov;
    for (int i = 6; i < 9; ++i)
        state_server.state_cov(i, i) = velocity_cov;
    for (int i = 9; i < 12; ++i)
        state_server.state_cov(i, i) = acc_bias_cov;
    for (int i = 15; i < 18; ++i)
        state_server.state_cov(i, i) = extrinsic_rotation_cov;
    for (int i = 18; i < 21; ++i)
        state_server.state_cov(i, i) = extrinsic_translation_cov;

    // Clear all exsiting features in the map.
    map_server.clear();

    // Clear the IMU msg buffer.
    imu_msg_buffer.clear();

    // Reset the starting flags.
    is_gravity_set = false;
    is_first_img = true;

    // Restart the subscribers.
    imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this);
    feature_sub = nh.subscribe("features", 40, &MsckfVio::featureCallback, this);

    // TODO: When can the reset fail?
    res.success = true;
    ROS_WARN("Resetting msckf vio completed...");
    return true;
}

4 featureCallback

/**
 * @brief 后端主要函数，处理新来的数据
 */
void MsckfVio::featureCallback(const CameraMeasurementConstPtr &msg)	// msg图像消息
{
	...
    // 1 IMU初始化判断
        
    // 2 IMU积分
    batchImuProcessing(msg->header.stamp.toSec());
    
}

4.1 IMU初始化判断

    // 1. 必须经过imu初始化
    if (!is_gravity_set)
        return;

    // Start the system if the first image is received. The frame where the first image is received will be the origin.
    // 开始递推状态的第一个时刻为初始化后的第一帧
    if (is_first_img)
    {
        is_first_img = false;
        state_server.imu_state.time = msg->header.stamp.toSec();
    }

4.2 IMU积分

遍历imu_msg_buffer容器中合法的IMU数据，对每一个数据进行processModel积分处理

4.2.1 batchImuProcessing

这个函数的主要功能就是筛选出两帧图像之间的imu消息去进行processModel处理。

在这里插入图片描述

/**
 * @brief imu积分，批量处理imu数据
 * @param  time_bound 处理到这个时间---就是当前新图像msg这个时间
 */
void MsckfVio::batchImuProcessing(const double &time_bound)
{
    // Counter how many IMU msgs in the buffer are used.
    int used_imu_msg_cntr = 0;

    // 取出两帧之间的imu数据去递推位姿
    // 这里有个细节问题，time_bound表示新图片的时间戳，
    // 但是IMU就积分到了距time_bound最近的一个，导致时间会差一点点
    for (const auto &imu_msg : imu_msg_buffer)
    {
        double imu_time = imu_msg.header.stamp.toSec();
        // 小于，说明这个数据比较旧，因为state_server.imu_state.time代表已经处理过的imu数据的时间
        if (imu_time < state_server.imu_state.time)
        {
            ++used_imu_msg_cntr;
            continue;
        }
        // 超过的供下次使用
        if (imu_time > time_bound)
            break;

        // Convert the msgs.
        Vector3d m_gyro, m_acc;
        tf::vectorMsgToEigen(imu_msg.angular_velocity, m_gyro);
        tf::vectorMsgToEigen(imu_msg.linear_acceleration, m_acc);

        // Execute process model.
        // 递推位姿，核心函数
        processModel(imu_time, m_gyro, m_acc);
        ++used_imu_msg_cntr;
    }

    // Set the state ID for the new IMU state.
    // 新的状态，更新id，相机状态的id也根据这个赋值
    state_server.imu_state.id = IMUState::next_id++;

    // 删掉已经用过
    // Remove all used IMU msgs.
    imu_msg_buffer.erase(
        imu_msg_buffer.begin(),
        imu_msg_buffer.begin() + used_imu_msg_cntr);

    return;
}

4.2.2 processModel


void MsckfVio::processModel(
    const double &time, const Vector3d &m_gyro, const Vector3d &m_acc)
{
    // 以引用的方式取出
    IMUState &imu_state = state_server.imu_state;

    // 1. imu读数减掉偏置
    Vector3d gyro = m_gyro - imu_state.gyro_bias;
    Vector3d acc = m_acc - imu_state.acc_bias;  // acc_bias 初始值是0
    double dtime = time - imu_state.time;
 
    // state_server.imu_state应该就是左边界！
    
    ...
        
    // 更新时间，time即当前处理的那个imu数据的时间戳！
    state_server.imu_state.time = time;
    return;    
}

所以，每一次调用这个函数，都是指两个imu数据之间的一个数据处理（预测等）。比如，dt时间内的状态预测，对于姿态，认为角速度dt时间内保持不变，即欧拉积分，直接利用time时刻即imu数据时刻得到的角速度w去预测这个时刻的位姿q。每计算完一个dt，就会更新时间state_server.imu_state.time = time。所以在观测来到前，所有IMU数据依次在上一个数据上预测。这也表明，如果没有引入外部的观测，这个预测的值一定会发散的！

在这里插入图片描述

① 移除imu测量偏置

/**
 * @brief 来一个新的imu数据更新协方差矩阵与状态积分
 * @param  time 新数据的时间戳
 * @param  m_gyro 角速度
 * @param  m_acc 加速度
 */
void MsckfVio::processModel(
    const double &time, const Vector3d &m_gyro, const Vector3d &m_acc)
{

    // 以引用的方式取出
    IMUState &imu_state = state_server.imu_state;

    // 1. imu读数减掉偏置
    Vector3d gyro = m_gyro - imu_state.gyro_bias;
    Vector3d acc = m_acc - imu_state.acc_bias;  // acc_bias 初始值是0
    double dtime = time - imu_state.time;
 
    // state_server.imu_state应该就是左边界！
    
    ...
        
    // 更新时间，time即当前处理的那个imu数据的时间戳
    state_server.imu_state.time = time;
    return;    
}

② 计算系数矩阵F和噪声矩阵G（连续，误差状态）

写成矩阵形式
$\dot{\delta\mathbf{x}}=\mathbf{F}_c\cdot\delta\mathbf{x}+\mathbf{G}_c\cdot\mathbf{n}$
变量：
$\left.\delta\mathbf{x}=\left(\begin{array}{cccccc}^I_G\delta\boldsymbol{\theta}^\top&\delta\mathbf{b}_g^\top&^G\delta\mathbf{v}_I^\top&\delta\mathbf{b}_a^\top&^G\delta\mathbf{p}_I^\top&{}^I_C\delta\boldsymbol{\theta}^\top&{}^I\delta\mathbf{p}_C^\top\end{array}\right.\right)^\top$

$\left.\mathbf{n}=\left(\begin{array}{ccc}\mathbf{n_g}&\mathbf{n_{wg}}&\mathbf{n_a}&\mathbf{n_{wa}}\end{array}\right.\right)^\top$

矩阵F注意点：

① 速度：即第三行，忽略了相关因素影响

② 外参：相机与IMU的转换，这个是不变的，所以第6、7行都是0，6、7列也是0，这里没写出来。

③ 这里只是5个变量，S-MSCKF论文以及代码中都还有外参两个自变量，所以F实际是21*21矩阵。7个变量，每个都是3×3

$\mathbf{F}=\begin{pmatrix}-\lfloor\hat{\boldsymbol{\omega}}_{\times}\rfloor&-\mathbf{I}_{3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\-C\left(\frac IC\hat{\mathbf{q}}\right)^\top\left\lfloor\hat{\mathbf{a}}_{\times}\right\rfloor&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&-C\left(\frac IG\hat{\mathbf{q}}\right)^\top&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{I}_{3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3} \end{pmatrix}$

矩阵G注意：原论文可能给出是下面式子，代码中有一个地方对不上，应该公式是写错了。只有位姿q、速度v、以及两个偏置b有噪声项。

G.block<3, 3>(9, 9) = Matrix3d::Identity();

$\left.\mathbf{G}=\left(\begin{array}{cccc}-\mathbf{I}_3&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{I}_3&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&-C\left(\frac IG\hat{\mathbf{q}}\right)^\top&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{I}_3\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\end{array}\right.\right)$


    // 2. 计算F G矩阵
    // Compute discrete transition and noise covariance matrix
    Matrix<double, 21, 21> F = Matrix<double, 21, 21>::Zero();
    Matrix<double, 21, 12> G = Matrix<double, 21, 12>::Zero();

    // 误差为真值（观测） - 预测
    // F矩阵表示的是误差的导数的微分方程，其实就是想求δ的递推公式
    // δ`= F · δ + G    δ表示误差
    // δn+1 = (I + F·dt)·δn + G·dt·Q    Q表示imu噪声

    // 两种推法，一种是通过论文中四元数的推，这个过程不再重复
    // 下面给出李代数推法，我们知道msckf的四元数使用的是反人类的jpl
    // 也就是同一个数值的旋转四元数经过两种不同定义得到的旋转是互为转置的
    // 这里面的四元数转成的旋转表示的是Riw，所以要以李代数推的话也要按照Riw推
    // 按照下面的旋转更新方式为左乘，因此李代数也要用左乘，且jpl模式下左乘一个δq = Exp(-δθ)
    // δQj * Qjw = Exp(-(w - b) * t) * δQi * Qiw
    // Exp(-δθj) * Qjw = Exp(-(w - b) * t) * Exp(-δθi) * Qiw
    // 其中Qjw = Exp(-(w - b) * t) * Qiw
    // 两边除得到 Exp(-δθj) = Exp(-(w - b) * t) * Exp(-δθi) * Exp(-(w - b) * t).t()
    // -δθj = - Exp(-(w - b) * t) * δθi
    // δθj = (I - (w - b)^ * t)  * δθi  得证

    // 关于偏置一样可以这么算，只不过式子变成了
    // δQj * Qjw = Exp(-(w - b - δb) * t) * Qiw
    // 上式使用bch近似公式可以得到 δθj = -t * δb
    // 其他也可以通过这个方法推出，是正确的

    F.block<3, 3>(0, 0) = -skewSymmetric(gyro);
    F.block<3, 3>(0, 3) = -Matrix3d::Identity();

									// imu_state.orientation---qbw  转置即qwb
    F.block<3, 3>(6, 0) =			// acc是imu坐标系下的，已经在①中减去了偏置
        -quaternionToRotation(imu_state.orientation).transpose() * skewSymmetric(acc);
    F.block<3, 3>(6, 9) = -quaternionToRotation(imu_state.orientation).transpose();
    F.block<3, 3>(12, 6) = Matrix3d::Identity();

    G.block<3, 3>(0, 0) = -Matrix3d::Identity();
    G.block<3, 3>(3, 3) = Matrix3d::Identity();
    G.block<3, 3>(6, 6) = -quaternionToRotation(imu_state.orientation).transpose();
    G.block<3, 3>(9, 9) = Matrix3d::Identity();

    // Approximate matrix exponential to the 3rd order,
    // which can be considered to be accurate enough assuming
    // dtime is within 0.01s.	这里主要是为了把连续时间矩阵F离散为状态转移矩阵，指数的泰勒3阶展开，再乘以时间dt
    Matrix<double, 21, 21> Fdt = F * dtime;				// 1阶近似
    Matrix<double, 21, 21> Fdt_square = Fdt * Fdt;		// 2阶近似
    Matrix<double, 21, 21> Fdt_cube = Fdt_square * Fdt;	// 3阶近似

把一个连续系统离散化，在线性系统理论中讲过，系统矩阵 $F$ 变为了状态转移矩阵 $\Phi(t+\Delta t,t)= exp(Ft)$
$\begin{aligned} \Phi(t+\Delta t,t)& =\exp\left(\mathbf{F}_{c}\Delta t\right) \\ &=\mathbf{I}_{21\times21}+\mathbf{F}_{c}\Delta t+\frac{1}{2!}\mathbf{F}_{c}^{2}\Delta t^{2}+\frac{1}{3!}\mathbf{F}_{c}^{3}\Delta t^{3} + ... \end{aligned}$
其中幂指数（注意这里F是简化版，道理是一样的）：
$\begin{aligned}&\mathbf{F}_c=\begin{bmatrix}-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor&-\mathbf{I}_{3\times3}\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\end{bmatrix}\quad,\quad\mathbf{F}_c^2=\begin{bmatrix}\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^2&\lfloor\hat{\boldsymbol{\omega}}\times\rfloor\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\end{bmatrix}\\&\mathbf{F}_c^3=\begin{bmatrix}-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^3&-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^2\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\end{bmatrix}\quad,\quad\mathbf{F}_c^4=\begin{bmatrix}\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^4&\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^3\\\mathbf{0}_{3\times3}&\mathbf{0}_{3\times3}\end{bmatrix}\end{aligned}$

③ 计算状态转移矩阵Phi（离散化的F，误差状态）

    // 3. 计算转移矩阵Phi矩阵
    Matrix<double, 21, 21> Phi =
        Matrix<double, 21, 21>::Identity() + Fdt +
        0.5 * Fdt_square + (1.0 / 6.0) * Fdt_cube;

④ IMU状态预测

注意这里是状态而不是误差状态！同样，我们要把连续时间下IMU状态运动学方程转换为离散时间下IMU运动学方程。为了更加精确的计算，对于速度v和位置p进行了四阶龙格库塔法积分。

下面是连续时间IMU运动学方程（微分方程），后面要离散化差分方程
$\begin{aligned} {^{I}_{G}\dot{\bar{q}}(t)}&=\frac{1}{2}\boldsymbol{\Omega}\big(\boldsymbol{\omega}(t)\big)_{G}^{I}\bar{q}(t),&\dot{\mathbf{b}}_{g}(t)&=\mathbf{n}_{wg}(t)\\\dot{\mathbf{v}}_{I}(t)&={}^{G}\mathbf{a}(t),&\dot{\mathbf{b}}_{a}(t)&=\mathbf{n}_{wa}(t),&\dot{\mathbf{p}}_{I}(t)&={}^{G}\mathbf{v}_{I}(t)\end{aligned}$

    // Propogate the state using 4th order Runge-Kutta
    // 4. 四阶龙格库塔积分预测状态
    predictNewState(dtime, gyro, acc);

<4.1> `Omega`矩阵

4.1 Omega矩阵用在向量和四元数的乘积中，例如用于四元数导数中

$\left.\boldsymbol{\Omega}(\boldsymbol{\omega})=\left[\begin{array}{cccc}0&\omega_z&-\omega_y&\omega_x\\-\omega_z&0&\omega_x&\omega_y\\\omega_y&-\omega_x&0&\omega_z\\-\omega_x&-\omega_y&-\omega_z&0\end{array}\right.\right] \left.=\left[\begin{array}{cc}-\lfloor\boldsymbol{\omega}\times\rfloor&\boldsymbol{\omega}\\-\boldsymbol{\omega}^\mathrm{T}&0\end{array}\right.\right]$

/**
 * @brief 来一个新的imu数据做积分，应用四阶龙哥库塔法
 * @param  dt 相对上一个数据的间隔时间
 * @param  gyro 角速度减去偏置后的
 * @param  acc 加速度减去偏置后的
 */
void MsckfVio::predictNewState(const double &dt, const Vector3d &gyro, const Vector3d &acc)
{

    // TODO: Will performing the forward integration using
    //    the inverse of the quaternion give better accuracy?

    // 角速度，标量
    double gyro_norm = gyro.norm();		// 用于选择离散公式
    // 4.1 计算Omega矩阵
    Matrix4d Omega = Matrix4d::Zero();
    Omega.block<3, 3>(0, 0) = -skewSymmetric(gyro);
    Omega.block<3, 1>(0, 3) = gyro;
    Omega.block<1, 3>(3, 0) = -gyro;
 
    ...
        
}

<4.2> 获取k时刻的imu状态

void MsckfVio::predictNewState(const double &dt, const Vector3d &gyro, const Vector3d &acc)
{
    ...
    
    // 4.2 获取k时刻的imu状态----注意这里是引用，所以后面预测会直接更改这里的变量。predictNewState处理之后imu_state就是k+1时刻的预测值了。
	Vector4d &q = state_server.imu_state.orientation;
    Vector3d &v = state_server.imu_state.velocity;
    Vector3d &p = state_server.imu_state.position;
  	
    ...
}

<4.3> 预测姿态 $_G^Iq^T$

采用论文Indirect Kalman Filter for 3D Attitude Estimation1.6节0阶离散时间差分方程（微分方程差分化）
$\begin{aligned}&|\hat{\omega}|>10^{-5}\text{ 时: }^I_G\hat{q}(t+\Delta t)=\left(\cos\left(\frac{|\hat{\omega}|}2\Delta t\right)\cdot I_{4\times4}+\frac1{|\hat{\omega}|}\sin\left(\frac{|\hat{\omega}|}2\Delta t\right)\cdot\Omega(\hat{\omega})\right) {^I_G\hat{q}}(t)\\&|\hat{\omega}|\leq10^{-5}\text{ 时: }^I_G\hat{q}(t+\Delta t)=\left(I_{4\times4}-\frac{\Delta t}2\Omega(\hat{\omega})\right){^I_G\hat{q}(t)}\end{aligned}$

    // Some pre-calculation
    // dq_dt表示积分n到n+1
    // dq_dt2表示积分n到n+0.5 算龙哥库塔用的
    Vector4d dq_dt, dq_dt2;
    if (gyro_norm > 1e-5)	// 角速度模长
    {
        dq_dt =
            (cos(gyro_norm * dt * 0.5) * Matrix4d::Identity() +
            1 / gyro_norm * sin(gyro_norm * dt * 0.5) * Omega) * q;
        dq_dt2 =
            (cos(gyro_norm * dt * 0.25) * Matrix4d::Identity() +
            1 / gyro_norm * sin(gyro_norm * dt * 0.25) * Omega) * q;
    }
    else
    {
        // 当角增量很小时的近似
        dq_dt = (Matrix4d::Identity() + 0.5 * dt * Omega) * cos(gyro_norm * dt * 0.5) * q;
        dq_dt2 = (Matrix4d::Identity() + 0.25 * dt * Omega) * cos(gyro_norm * dt * 0.25) * q;
    }
    // Rwi  转换为旋转矩阵
    Matrix3d dR_dt_transpose = quaternionToRotation(dq_dt).transpose();
    Matrix3d dR_dt2_transpose = quaternionToRotation(dq_dt2).transpose();

    q = dq_dt;
    quaternionNormalize(q);		// 一定要记得归一化！

<4.4> 预测速度v和位置p

四阶龙格库塔法

$\left.\left\{\begin{array}{l}y_{n+1}=y_n+\frac h6\left(k_1+2\boldsymbol{k}_2+2k_3+k_4\right)\\k_1=f\left(x_n,y_n\right)\\k_2=f\left(x_n+\frac h2,y_n+\frac h2k_1\right)\\k_3=f\left(x_n+\frac\hbar2,y_n+\frac\hbar2\boldsymbol{k}_2\right)\\\boldsymbol{k}_4=\boldsymbol{f}\left(\boldsymbol{x}_n+\boldsymbol{h},\boldsymbol{y}_n+\boldsymbol{h}\boldsymbol{k}_3\right)\end{array}\right.\right.$

利用龙格库塔法预测速度v和位移p

$\begin{aligned} &\hat{v}(t+\Delta t)=\hat{v}(t)+\frac{\Delta t}6\left(k_{v_1}+2k_{v_2}+2k_{v_3}+k_{v_4}\right) \\ &k_{v_1}=\hat{R}(t)\hat{a}+g \\ &k_{v_2}=\hat{R}(t+\Delta t/2)\hat{a}+g \\ &k_{v_3}=\hat{R}(t+\Delta t/2)\hat{a}+g \\ &k_{v_4}=\hat{R}(t+\Delta t)\hat{a}+g \\ &\hat{p}(t+\Delta t)=\hat{p}(t)+\frac{\Delta t}6\left(k_{p_1}+2k_{p_2}+2k_{p_3}+k_{p_4}\right) \\ &k_{p_1}=\hat{v}(t) \\ &k_{p_2}=\hat{v}(t)+k_{v_1}\Delta t/2 \\ &k_{p_3}=\hat{v}(t)+k_{v_2}\Delta t/2 \\ &k_{p_4}=\hat{v}(t)+k_{v_3}\Delta t \end{aligned}$

    // k1 = f(tn, yn)  ----kv1
    Vector3d k1_v_dot = quaternionToRotation(q).transpose() * acc + IMUState::gravity;
    Vector3d k1_p_dot = v;		// kp1

    // k2 = f(tn+dt/2, yn+k1*dt/2)
    // 这里的4阶LK法用了匀加速度假设，即认为前一时刻的加速度和当前时刻相等
    Vector3d k1_v = v + k1_v_dot * dt / 2;
    Vector3d k2_v_dot = dR_dt2_transpose * acc + IMUState::gravity;
    Vector3d k2_p_dot = k1_v;

    // k3 = f(tn+dt/2, yn+k2*dt/2)
    Vector3d k2_v = v + k2_v_dot * dt / 2;
    Vector3d k3_v_dot = dR_dt2_transpose * acc + IMUState::gravity;
    Vector3d k3_p_dot = k2_v;

    // k4 = f(tn+dt, yn+k3*dt)
    Vector3d k3_v = v + k3_v_dot * dt;
    Vector3d k4_v_dot = dR_dt_transpose * acc + IMUState::gravity;
    Vector3d k4_p_dot = k3_v;

    // yn+1 = yn + dt/6*(k1+2*k2+2*k3+k4)
    v = v + dt / 6 * (k1_v_dot + 2 * k2_v_dot + 2 * k3_v_dot + k4_v_dot);
    p = p + dt / 6 * (k1_p_dot + 2 * k2_p_dot + 2 * k3_p_dot + k4_p_dot);

    return;
}

⑤ 可观性约束

这部分不好理解，但公式可以对上。

$\mathbf{N}_{r,k+1}=\mathbf{\Phi}_k\mathbf{N}_{r,k}\quad\to\quad\begin{bmatrix}\mathbf{C}\left(\hat{q}_{c,k+1|k}\right)^{c}\mathbf{g}\\\mathbf{0}_{3}\\-\lfloor c\hat{\mathbf{v}}_{t,k+1|k}\times\rfloor^{c}\mathbf{g}\\\mathbf{0}_{3}\\-\lfloor{}^{c}\hat{\mathbf{p}}_{t,k+1|k}\times\rfloor^{c}\mathbf{g}\end{bmatrix}=\begin{bmatrix}\Phi_{11}&\Phi_{12}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}\\\mathbf{0}_{3}&\mathbf{I}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}\\\Phi_{31}&\Phi_{32}&\mathbf{I}_{3}&\mathbf{0}_{34}&\mathbf{0}_{3}\\\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{I}_{3}&\mathbf{0}_{3}\\\mathbf{0}_{51}&\Phi_{52}&\delta t\mathbf{I}_{3}&\Phi_{54}&\mathbf{I}_{3}\end{bmatrix}\begin{bmatrix}\mathbf{C}\left(\hat{q}_{c,k|k-1}\right)^{c}\mathbf{g}\\\mathbf{0}_{3}\\-\lfloor\hat{q}_{t,k|k-1}\times\rfloor^{c}\mathbf{g}\\\mathbf{0}_{3}\\\mathbf{0}_{3}\end{bmatrix}.$
我们把这个矩阵乘出来，能得到3个等式（主要是修改Φ11，31，51）：

第1行：可以直接接出来

$\mathbf{C}\left({}^{\prime}\hat{\bar{q}}_{G,k+1|k}\right)^{c}\mathbf{g}=\Phi_{11}\mathbf{C}\left({}^{\prime}\hat{\bar{q}}_{G,k|k-1}\right)^{c}\mathbf{g}\quad\to\Phi_{11}=\mathbf{C}\left({}^{\iota,k+1|k}\hat{\tilde{q}}_{\iota,k|k-1}\right).$

第3、5行：论文中提到线性相关，无法直接求，或者说由多个解（等式中包含了反对
称矩阵导致实际右侧如果按照第⼀个式子那么算就会导致得到矩阵是线性相关的）

$\begin{aligned}\Phi_{31}\mathbf{C}\left(\hat{q}_{c,k|k-1}\right)^G\mathbf{g}&=\lfloor{}^G\hat{\mathbf{v}}_{l,k|k-1}\times\rfloor^G\mathbf{g}-\lfloor{}^G\hat{\mathbf{v}}_{l,k+1|k}\times\rfloor^G\mathbf{g}\\\Phi_{51}\mathbf{C}\left({}^I\hat{q}_{G,k|k-1}\right)^G\mathbf{g}&=\delta t\lfloor{}^G\hat{\mathbf{v}}_{l,k|k-1}\times\rfloor^G\mathbf{g}-\lfloor{}^G\hat{\mathbf{p}}_{l,k+1|k}\times\rfloor^G\mathbf{g}\end{aligned}$

然后论文里面引入了一个最小二乘类似的约束，其中 A=Φ31 或 Φ51

$\min_{\mathbf{A}^{*}}\left\|\mathbf{A}^{*}-\mathbf{A}\right\|_{\mathcal{F}}^{2},\quad\text{s.t. }\mathbf{A}^{*}\mathbf{u}=\mathbf{w}$

$\mathbf{A}^*=\mathbf{A}-(\mathbf{A}\mathbf{u}-\mathbf{w})\left(\mathbf{u}^T\mathbf{u}\right)^{-1}\mathbf{u}^T$

    // 5. Observability-constrained VINS 可观性约束
    // Modify the transition matrix
    // 5.1 修改phi_11
    // imu_state.orientation_null为上一个imu数据递推后保存的
    // 这块可能会有疑问，因为当上一个imu假如被观测更新了，
    // 导致当前的imu状态是由更新后的上一个imu状态递推而来，但是这里的值是没有更新的，这个有影响吗
    // 答案是没有的，因为我们更改了phi矩阵，保证了零空间
    // 并且这里必须这么处理，因为如果使用更新后的上一个imu状态构建上一时刻的零空间
    // 就破坏了上上一个跟上一个imu状态之间的0空间
    // Ni-1 = phi_[i-2] * Ni-2
    // Ni = phi_[i-1] * Ni-1^
    // 如果像上面这样约束，那么中间的0空间就“崩了”
    Matrix3d R_kk_1 = quaternionToRotation(imu_state.orientation_null);
    Phi.block<3, 3>(0, 0) =
        quaternionToRotation(imu_state.orientation) * R_kk_1.transpose();

    // 5.2 修改phi_31
    Vector3d u = R_kk_1 * IMUState::gravity;
    RowVector3d s = (u.transpose() * u).inverse() * u.transpose();

    Matrix3d A1 = Phi.block<3, 3>(6, 0);
    Vector3d w1 =
        skewSymmetric(imu_state.velocity_null - imu_state.velocity) * IMUState::gravity;
    Phi.block<3, 3>(6, 0) = A1 - (A1 * u - w1) * s;

    // 5.3 修改phi_51
    Matrix3d A2 = Phi.block<3, 3>(12, 0);
    Vector3d w2 =
        skewSymmetric(
            dtime * imu_state.velocity_null + imu_state.position_null -
            imu_state.position) *
        IMUState::gravity;
    Phi.block<3, 3>(12, 0) = A2 - (A2 * u - w2) * s;

⑥ 更新0空间约束后的状态转移矩阵phi和误差状态协方差矩阵

$\begin{gathered}\mathbf{P}_{k+1|k}=\Phi_k\mathbf{P}_{k|k}\Phi_k^T+\mathbf{Q}_{d,k} \\ \mathbf{Q}_{d,k}=\int_{t_k}^{t_{k+1}}\boldsymbol{\Phi}\left(t_{k+1},\tau\right)\mathbf{G}_c(\tau)\mathbf{Q}_c\mathbf{G}_c^\mathrm{T}(\tau)\boldsymbol{\Phi}^\mathrm{T}\left(t_{k+1},\tau\right)\mathrm{d}\tau\end{gathered}\\$

    // Propogate the state covariance matrix.
    // 6. 使用0空间约束后的phi计算积分后的新的协方差矩阵
	// Q是高斯白噪声带来的协方差矩阵，代码中简单计算了
    Matrix<double, 21, 21> Q =
        Phi * G * state_server.continuous_noise_cov * G.transpose() * Phi.transpose() * dtime;	// G就是上面求得F、G

	// 误差状态协方差矩阵
    state_server.state_cov.block<21, 21>(0, 0) =
        Phi * state_server.state_cov.block<21, 21>(0, 0) * Phi.transpose() + Q;

⑦ 更新imu状态量与相机状态量交叉的部分

$\left.\mathbf{P}_{k+1|k}=\left(\begin{matrix}\mathbf{P}_{II_{k+1|k}}&\mathbf{\Phi}_{k}\mathbf{P}_{IC_{k|k}}\\\mathbf{P}_{IC_{k|k}}^{\top}\mathbf{\Phi}_{k}^{\top}&\mathbf{P}_{CC_{k|k}}\end{matrix}\right.\right)$
在这里插入图片描述

    // 7. 如果有相机状态量，那么更新imu状态量与相机状态量交叉的部分
    if (state_server.cam_states.size() > 0)
    {
        // 起点是0 21  然后是21行 state_server.state_cov.cols() - 21 列的矩阵
        // 也就是整个协方差矩阵的右上角，这部分说白了就是imu状态量与相机状态量的协方差，imu更新了，这部分也需要更新
        state_server.state_cov.block(0, 21, 21, state_server.state_cov.cols() - 21) =
            Phi * state_server.state_cov.block(0, 21, 21, state_server.state_cov.cols() - 21);

        // 同理，这个是左下角
        state_server.state_cov.block(21, 0, state_server.state_cov.rows() - 21, 21) =
            state_server.state_cov.block(21, 0, state_server.state_cov.rows() - 21, 21) *
            Phi.transpose();
    }

⑧ 强制对称协方差矩阵

    // 8. 强制对称，因为协方差矩阵就是对称的
    MatrixXd state_cov_fixed = 
        (state_server.state_cov + state_server.state_cov.transpose()) / 2.0;
    state_server.state_cov = state_cov_fixed;

⑨ 更新零空间


    // Update the state correspondes to null space.
    // 9. 更新零空间，供下个IMU来了使用---
	// imu_state.orientation这种目前就是上一个时刻状态预测值！
    imu_state.orientation_null = imu_state.orientation;
    imu_state.position_null = imu_state.position;
    imu_state.velocity_null = imu_state.velocity;

    // 更新时间
    state_server.imu_state.time = time;
    return;
}

4.3 状态增广 stateAugmentation

在没有图像进来时，对IMU状态进行预测，并计算系统误差状态协方差矩阵；在有图像进来时，根据相机与IMU的相对外参计算当前相机的位姿。然后将最新的相机状态加入到系统状态向量中去，然后扩增误差状态协方差矩阵。

4.3.1 利用最新的imu状态计算cam状态

const CameraMeasurementConstPtr &msg = msg->header.stamp.toSec()前最近的1次IMU预测的状态。理论上和相机时间戳是差了一点的，但无伤大雅，近似为同一时间。

When new images are received, the state should be augmented with the new camera state. The pose of the new camera state can be computed from the latest IMU state as
$\left._G^C\hat{\mathbf{q}}={}_I^C\hat{\mathbf{q}}\otimes{}_G^I\hat{\mathbf{q}},\quad{}^G\hat{\mathbf{p}}_C={}^G\hat{\mathbf{p}}_C+C\left(\begin{matrix}^I_G\hat{\mathbf{q}}\end{matrix}\right.\right)^\top{}^I\hat{\mathbf{p}}_C$

/**
 * @brief 根据时间分裂出相机状态
 * @param  time 图片的时间戳
 */
void MsckfVio::stateAugmentation(const double &time)
{
    // 1. 取出当前更新好的imu状态量
    // 1.1 取出状态量中的外参，imu到cam的外参
    const Matrix3d &R_i_c = state_server.imu_state.R_imu_cam0;
    const Vector3d &t_c_i = state_server.imu_state.t_cam0_imu;

    // Add a new camera state to the state server.
    // 1.2 取出imu旋转平移，按照外参，将这个时刻cam0的位姿算出来
    Matrix3d R_w_i = quaternionToRotation(
        state_server.imu_state.orientation);
    Matrix3d R_w_c = R_i_c * R_w_i;			//Rcw
    Vector3d t_c_w = state_server.imu_state.position +	// twc
                        R_w_i.transpose() * t_c_i;
    

}

4.3.2 记录相机状态- `imu_state.id`

    // 2. 注册新的相机状态到状态库中
    // 嗯。。。说人话就是找个记录的，不然咋更新
    state_server.cam_states[state_server.imu_state.id] =
        CAMState(state_server.imu_state.id);
    CAMState &cam_state = state_server.cam_states[state_server.imu_state.id];

    // 严格上讲这个时间不对，但是几乎没影响 ---cam时间略大于imu时间
    cam_state.time = time;
    cam_state.orientation = rotationToQuaternion(R_w_c);
    cam_state.position = t_c_w;

    // 记录第一次被估计的数据，不能被改变，因为改变了就破坏了之前的0空间
    cam_state.orientation_null = cam_state.orientation;
    cam_state.position_null = cam_state.position;

4.3.3 更新协方差矩阵

① 计算雅可比矩阵

6*(21+6N)

$\mathbf{J}=\begin{pmatrix}\mathbf{J}_I&\mathbf{0}_{6\times6N}\end{pmatrix}$

6*21

$\left.J_I=\left[\begin{array}{cccccccc}R_{cb}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}\\(R_{wb}t_{bc})^{\wedge}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}&R_{wb}\end{array}\right.\right]$

    // 3. 这个雅可比可以认为是cam0位姿相对于imu的状态量的求偏导
    // 此时我们首先要知道相机位姿是 Rcw  twc
    // Rcw = Rci * Riw   twc = twi + Rwi * tic
    Matrix<double, 6, 21> J = Matrix<double, 6, 21>::Zero();
    // Rcw对Riw的左扰动导数
    J.block<3, 3>(0, 0) = R_i_c;
    // Rcw对Rci的左扰动导数
    J.block<3, 3>(0, 15) = Matrix3d::Identity();

    // twc对Riw的左扰动导数
    // twc = twi + Rwi * Exp(φ) * tic
    //     = twi + Rwi * (I + φ^) * tic
    //     = twi + Rwi * tic + Rwi * φ^ * tic
    //     = twi + Rwi * tic - Rwi * tic^ * φ
    // 这部分的偏导为 -Rwi * tic^     与论文一致
    // TODO 试一下 -R_w_i.transpose() * skewSymmetric(t_c_i)
    // 其实这里可以反过来推一下当给的扰动是
    // twc = twi + Exp(-φ) * Rwi * tic
    //     = twi + (I - φ^) * Rwi * tic
    //     = twi + Rwi * tic - φ^ * Rwi * tic
    //     = twi + Rwi * tic + (Rwi * tic)^ * φ
    // 这样跟代码就一样了，但是上下定义的扰动方式就不同了
    J.block<3, 3>(3, 0) = skewSymmetric(R_w_i.transpose() * t_c_i);
    // 下面是代码里自带的，论文中给出的也是下面的结果
    // J.block<3, 3>(3, 0) = -R_w_i.transpose()*skewSymmetric(t_c_i);

    // twc对twi的左扰动导数
    J.block<3, 3>(3, 12) = Matrix3d::Identity();
    // twc对tic的左扰动导数
    J.block<3, 3>(3, 18) = R_w_i.transpose();

② 计算增广协方差矩阵

就是说原来只有左上角的P，现在多了其余三个部分！整体来看就是整个矩阵的行维和列维都多了6！(old_rows + 6, old_cols + 6)代码也能看出来。
$\begin{gathered} P^{(21+6(N+1))\times(21+6(N+1))} \left.=\left[\begin{array}{c}I_{21+6N}\\J_{6\times(21+6N)}\end{array}\right.\right]P^{(21+6N)\times(21+6N)}\left[\begin{array}{c}I_{21+6N}\\J_{6\times(21+6N)}\end{array}\right]^T \\ \left.=\left[\begin{array}{cc}P&PJ^T\\JP&JPJ^T\end{array}\right.\right] \end{gathered}$

    // 4. 增广协方差矩阵
    // 简单地说就是原来的协方差是 21 + 6n 维的，现在新来了一个伙计，维度要扩了
    // 并且对应位置的值要根据雅可比跟这个时刻（也就是最新时刻）的imu协方差计算
    // 4.1 扩展矩阵大小 conservativeResize函数不改变原矩阵对应位置的数值-----左上角不需要改变
    // Resize the state covariance matrix.
    size_t old_rows = state_server.state_cov.rows();
    size_t old_cols = state_server.state_cov.cols();
    state_server.state_cov.conservativeResize(old_rows + 6, old_cols + 6);

    // Rename some matrix blocks for convenience.
    // imu的协方差矩阵
    const Matrix<double, 21, 21> &P11 =
        state_server.state_cov.block<21, 21>(0, 0);

    // imu相对于各个相机状态量的协方差矩阵（不包括最新的）--对过去相机状态量的雅可比。就是上一次协方差的右上角！
    const MatrixXd &P12 =
        state_server.state_cov.block(0, 21, 21, old_cols - 21);


    // 4.2 计算协方差矩阵
    // 左下角
    state_server.state_cov.block(old_rows, 0, 6, old_cols) << J * P11, J * P12;

    // 右上角-----（21+6N）*6---与左下角互为转置
    state_server.state_cov.block(0, old_cols, old_rows, 6) =
        state_server.state_cov.block(old_rows, 0, 6, old_cols).transpose();

    // 右下角，关于相机部分的J都是0所以省略了----6*6矩阵
    state_server.state_cov.block<6, 6>(old_rows, old_cols) =
        J * P11 * J.transpose();

    // 强制对称
    MatrixXd state_cov_fixed = (state_server.state_cov +
                                state_server.state_cov.transpose()) /
                                2.0;
    state_server.state_cov = state_cov_fixed;

    return;
}

4.4 添加特征点观测

现在来了一帧新的图像，那么就会有新的特征点，所以把要添加新的特征信息。

/**
 * @brief 添加特征点观测
 * @param  msg 前端发来的特征点信息，里面包含了时间，左右目上的角点及其id（严格意义上不能说是特征点）
 */
void MsckfVio::addFeatureObservations(
    const CameraMeasurementConstPtr &msg)
{
    // 这是个long long int 嗯。。。。直接当作int理解吧
    // 这个id会在 batchImuProcessing 更新
    StateIDType state_id = state_server.imu_state.id;

    // 1. 获取当前窗口内特征点数量
    int curr_feature_num = map_server.size();
    int tracked_feature_num = 0;

    // Add new observations for existing features or new
    // features in the map server.
    // 2. 添加新来的点，做的花里胡哨，其实就是在现有的特征管理里面找，
    // id已存在说明是跟踪的点，在已有的上面更新
    // id不存在说明新来的点，那么就新添加一个
    for (const auto &feature : msg->features)
    {
        if (map_server.find(feature.id) == map_server.end())
        {
            // This is a new feature.
            map_server[feature.id] = Feature(feature.id);
            map_server[feature.id].observations[state_id] =
                Vector4d(feature.u0, feature.v0,
                            feature.u1, feature.v1);
        }
        else
        {
            // This is an old feature.
            map_server[feature.id].observations[state_id] =
                Vector4d(feature.u0, feature.v0,
                            feature.u1, feature.v1);
            ++tracked_feature_num;
        }
    }

    // 这个东西计算了当前进来的跟踪的点中在总数里面的占比（进来的点有可能是新提的）
    tracking_rate =
        static_cast<double>(tracked_feature_num) /
        static_cast<double>(curr_feature_num);

    return;
}

4.5 利用视觉观测更新状态

removeLostFeatures()

4.5.1 特征点管理

这部分与cam有了较大的联系，看代码之前要搞清楚下面几个变量。

状态量	含义
`map_server<FeatureIDType, Feature>`	存储满足要求的特征点的`map`容器，键-特征点id，值-对应特征点
`FeatureIDType`	长整型`long long int`
`feature.observations<StateIDType, Eigen::Vector4d>`	哪些帧观测到了这个特征点，相应的归一化坐标是什么？

这里主要做了两个工作，一个是删除那么无效点，一个是找到哪些当前已经观测不到的有效点去进行后续优化。

/**
 * @brief 使用不再跟踪上的点来更新
 */
void MsckfVio::removeLostFeatures()
{
    // 移除哪些追踪丢失的点
    // BTW, find the size the final Jacobian matrix and residual vector.
    int jacobian_row_size = 0;

    vector<FeatureIDType> invalid_feature_ids(0);  // 无效点，最后要删的
    vector<FeatureIDType> processed_feature_ids(0);  // 待参与更新的点，用完也被无情的删掉

    // 遍历所有特征管理里面的点，包括新进来的
    for (auto iter = map_server.begin();
            iter != map_server.end(); ++iter)
    {
        // Rename the feature to be checked.
        // 引用，改变feature相当于改变iter->second，类似于指针的效果
        auto &feature = iter->second;

        // 1. 这个点被当前状态观测到，说明这个点后面还有可能被跟踪
        // 跳过这些点
        if (feature.observations.find(state_server.imu_state.id) !=
            feature.observations.end())
            continue;

        // 2. 跟踪小于3帧的点，认为是质量不高的点
        // 也好理解，三角化起码要两个观测，但是只有两个没有其他观测来验证
        if (feature.observations.size() < 3)
        {
            invalid_feature_ids.push_back(feature.id);
            continue;
        }

        // 3. 如果这个特征没有被初始化，尝试去初始化
        // 初始化就是三角化
        if (!feature.is_initialized)
        {
            // 3.1 看看运动是否足够，没有足够视差或者平移小旋转多这种不符合三角化
            // 所以就不要这些点了
            if (!feature.checkMotion(state_server.cam_states))
            {
                invalid_feature_ids.push_back(feature.id);
                continue;
            }
            else
            {
                // 3.3 尝试三角化，失败也不要了
                if (!feature.initializePosition(state_server.cam_states))
                {
                    invalid_feature_ids.push_back(feature.id);
                    continue;
                }
            }
        }

        // 4. 到这里表示这个点能用于更新，所以准备下一步计算
        // 一个观测代表一帧，一帧有左右两个观测
        // 也就是算重投影误差时维度将会是4 * feature.observations.size()
        // 这里为什么减3下面会提到
        jacobian_row_size += 4 * feature.observations.size() - 3;
        // 接下来要参与优化的点加入到这个变量中
        processed_feature_ids.push_back(feature.id);
    }


    // Remove the features that do not have enough measurements.
    // 5. 删掉非法点
    for (const auto &feature_id : invalid_feature_ids)
        map_server.erase(feature_id);
    
    // Return if there is no lost feature to be processed.
    if (processed_feature_ids.size() == 0)
        return;

4.5.2 计算误差量雅可比与重投影误差

    // 准备好误差相对于状态量的雅可比
    MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
                                    21 + 6 * state_server.cam_states.size());
    VectorXd r = VectorXd::Zero(jacobian_row_size);
    int stack_cntr = 0;


    // 6. 处理特征点
    for (const auto &feature_id : processed_feature_ids)
    {
        auto &feature = map_server[feature_id];

        vector<StateIDType> cam_state_ids(0);
        for (const auto &measurement : feature.observations)
            cam_state_ids.push_back(measurement.first);

        MatrixXd H_xj;
        VectorXd r_j;
        // 6.1 计算雅可比，计算重投影误差
        featureJacobian(feature.id, cam_state_ids, H_xj, r_j);

        // 6.2 卡方检验，剔除错误点，并不是所有点都用
        if (gatingTest(H_xj, r_j, cam_state_ids.size() - 1))
        {
            H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
            r.segment(stack_cntr, r_j.rows()) = r_j;
            stack_cntr += H_xj.rows();
        }

        // Put an upper bound on the row size of measurement Jacobian,
        // which helps guarantee the executation time.
        // 限制最大更新量
        if (stack_cntr > 1500)
            break;
    }

① featureJacobian()

一个特征观测误差对于位姿的雅可比是4*6，对于地图点的雅可比是4*3.因为是双目，所以2*2共4维行向量，每一个误差都是归一化后的坐标x、y组成
$\begin{gathered} \mathrm{H}_{C_{i}}^{j} =\frac{\partial\mathbf{z}_{i}^{j}}{\partial^{C_{i,1}}\mathbf{p}_{j}}\cdot\frac{\partial^{C_{i,1}}\mathbf{p}_{j}}{\partial\mathbf{x}_{C_{i,1}}}+\frac{\partial\mathbf{z}_{i}^{j}}{\partial^{C_{i,2}}\mathbf{p}_{j}}\cdot\frac{\partial^{C_{i,2}}\mathbf{p}_{j}}{\partial\mathbf{x}_{C_{i,1}}} \\ \mathbf{H}_{f_{i}}^{j} =\frac{\partial\mathbf{z}_{i}^{j}}{\partial^{C_{i,1}}\mathbf{p}_{j}}\cdot\frac{\partial^{C_{i,1}}\mathbf{p}_{j}}{\partial^{G}\mathbf{p}_{j}}+\frac{\partial\mathbf{z}_{i}^{j}}{\partial^{C_{i,2}}\mathbf{p}_{j}}\cdot\frac{\partial^{C_{i,2}}\mathbf{p}_{j}}{\partial^{G}\mathbf{p}_{j}} \end{gathered}$
整体的求雅可比过程和十四讲一致，都是利用链式法则，误差量先对相机系下点 $^CP(x,y,z)$ 求导，然后点 $^CP(x,y,z)$ 再分别对位姿和地图点求导。

求雅可比的关键就是：①搞清楚误差量是什么 ②搞清楚优化变量是什么（一般是位姿和路标点）

还有一个值得注意的点就是，这里平移量的不同表达，会使得雅可比的结果略有不同，如下所示（l表示做相机）
$^CP = R_{lw}(P_{w} - t_{wl}) = R_{lw}P_{w} + t_{lw}$
后面那种表达就是十四讲上经常使用的方法，推导对于位姿和地图点的导数也和上面一致。但是MSCKF中使用的是前面一种表达，两种表达本质上一样，但对平移量的表示不一致，所以会导致对于位姿中平移量的雅可比会有差异（很明显，点 $^CP$ 分别对两个平移量求导，一个是 $R_{lw}$ ，一个是单位矩阵 $I$ ）。

当然还有一点，就是MSCKF中都是jpl四元数，所有的李代数扰动都是负的。

/**
 * @brief 计算一个路标点的雅可比
 * @param  feature_id 路标点id
 * @param  cam_state_ids 这个点对应的所有的相机状态id
 * @param  H_x 雅可比
 * @param  r 误差
 */
void MsckfVio::featureJacobian(
    const FeatureIDType &feature_id,
    const std::vector<StateIDType> &cam_state_ids,
    MatrixXd &H_x, VectorXd &r)
{
    // 取出特征
    const auto &feature = map_server[feature_id];

    // Check how many camera states in the provided camera
    // id camera has actually seen this feature.
    // 1. 统计有效观测的相机状态，因为对应的个别状态有可能被滑走了
    vector<StateIDType> valid_cam_state_ids(0);
    for (const auto &cam_id : cam_state_ids)
    {
        if (feature.observations.find(cam_id) ==
            feature.observations.end())
            continue;

        valid_cam_state_ids.push_back(cam_id);
    }

    int jacobian_row_size = 0;
    // 行数等于4*观测数量，一个观测在双目上都有，所以是2*2
    // 此时还没有0空间投影
    jacobian_row_size = 4 * valid_cam_state_ids.size();

    // 误差相对于状态量的雅可比，没有约束列数，因为列数一直是最新的
    // 21+6N---21是imu状态、外参、N表示了多少个
    MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size,
                                    21 + state_server.cam_states.size() * 6);
    // 误差相对于三维点的雅可比
    MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3);
    // 误差
    VectorXd r_j = VectorXd::Zero(jacobian_row_size);
    int stack_cntr = 0;

    // 2. 计算每一个观测（同一帧左右目这里被叫成一个观测）的雅可比与误差
    for (const auto &cam_id : valid_cam_state_ids)
    {
		// 一个观测对位姿雅可比-4*6  对地图点的雅可比--4*3
        Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
        Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero();
        Vector4d r_i = Vector4d::Zero();
        // 2.1 计算一个左右目观测的雅可比
        measurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i);

        // 计算这个cam_id在整个矩阵的列数，因为要在大矩阵里面放
        auto cam_state_iter = state_server.cam_states.find(cam_id);
        int cam_state_cntr = std::distance(
            state_server.cam_states.begin(), cam_state_iter);

        // Stack the Jacobians.
        H_xj.block<4, 6>(stack_cntr, 21 + 6 * cam_state_cntr) = H_xi;
        H_fj.block<4, 3>(stack_cntr, 0) = H_fi;
        r_j.segment<4>(stack_cntr) = r_i;
        stack_cntr += 4;
    }

② measurementJacobian()

误差量对相机系下点的导数，以左相机 $C_{i,1}$ 为例

$\left.\frac{\partial\mathbf{z}_i^j}{\partial^{C_{i,1}}\mathbf{p}_j}=\frac{1}{C_{i,1}\hat{Z}_j}\left(\begin{array}{ccc}1&0&-\frac{C_{i,1}\hat{X}_j}{C_{i,1}\hat{Z}_j}\\0&1&-\frac{C_{i,1}\hat{Y}_j}{C_{i,1}\hat{Z}_j}\\0&0&0\\0&0&0\end{array}\right.\right)$

相机系点对位姿的雅可比

$\left.\left.\frac{\partial^{C_{i,1}}\mathbf{p}_j}{\partial\mathbf{x}_{C_{i,1}}}=\left(\left.\lfloor C_{i,1}\hat{\mathbf{p}}_{j\times}\rfloor\quad-C\left(\begin{matrix}C_{i,1}\\G\end{matrix}\right.\right.\right.\hat{\mathbf{q}}\right)\right)$

相机系点对地图点的雅可比

$\frac{\partial^{C_{i,1}}\mathbf{p}_j}{\partial^G\mathbf{p}_j}=\left.C\left(\begin{matrix}C_i,1\\G\end{matrix}\right.\mathbf{\hat{q}}\right)$

<1> 取出观测z和位姿Rwc、路标点p_w

/**
 * @brief 计算一个路标点的雅可比
 * @param  cam_state_id 有效的相机状态id
 * @param  feature_id 路标点id
 * @param  H_x 误差相对于位姿的雅可比
 * @param  H_f 误差相对于三维点的雅可比
 * @param  r 误差
 */
void MsckfVio::measurementJacobian(
    const StateIDType &cam_state_id,
    const FeatureIDType &feature_id,
    Matrix<double, 4, 6> &H_x, Matrix<double, 4, 3> &H_f, Vector4d &r)
{

    // 1. 取出相机状态与特征
    const CAMState &cam_state = state_server.cam_states[cam_state_id];
    const Feature &feature = map_server[feature_id];

    // 2. 取出左目位姿，根据外参计算右目位姿
    // Cam0 pose.	Rc0w
    Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
    const Vector3d &t_c0_w = cam_state.position;	// twc0

    // Cam1 pose.	
    Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
     // Rc1w
    Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
    Vector3d t_c1_w = t_c0_w - R_w_c1.transpose() * CAMState::T_cam0_cam1.translation();	// twc1= twc0-Rwc1*tc1c0

    // 3. 取出三维点坐标--地图点与观测值z(前端发来的是归一化坐标)
    const Vector3d &p_w = feature.position;	// 这个特征对于世界系地图点坐标
    const Vector4d &z = feature.observations.find(cam_state_id)->second;	// 这个特征在当前观测帧下的归一化坐标-----观测值

<2> 计算雅可比H

    // 4. 转到左右目相机坐标系下
    // Convert the feature position from the world frame to
    // the cam0 and cam1 frame.
    Vector3d p_c0 = R_w_c0 * (p_w - t_c0_w);
    Vector3d p_c1 = R_w_c1 * (p_w - t_c1_w);
    // p_c1 = R_c0_c1 * R_w_c0 * (p_w - t_c0_w + R_w_c1.transpose() * t_cam0_cam1)
    //      = R_c0_c1 * (p_c0 + R_w_c0 * R_w_c1.transpose() * t_cam0_cam1)
    //      = R_c0_c1 * (p_c0 + R_c0_c1 * t_cam0_cam1)


    // 5. 计算雅可比
    // 左相机归一化坐标点（误差量）相对于左相机坐标系下的点的雅可比
    // (x, y) = (X / Z, Y / Z)
    Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
    dz_dpc0(0, 0) = 1 / p_c0(2);
    dz_dpc0(1, 1) = 1 / p_c0(2);
    dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2) * p_c0(2));
    dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2) * p_c0(2));

    // 与上同理
    Matrix<double, 4, 3> dz_dpc1 = Matrix<double, 4, 3>::Zero();
    dz_dpc1(2, 0) = 1 / p_c1(2);
    dz_dpc1(3, 1) = 1 / p_c1(2);
    dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2) * p_c1(2));
    dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2) * p_c1(2));

    // 左相机坐标系下的三维点相对于左相机位姿的雅可比 先r后t
    Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
    dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
    dpc0_dxc.rightCols(3) = -R_w_c0;

    // 右相机坐标系下的三维点相对于左相机位姿的雅可比 先r后t
    Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
    dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0);
    dpc1_dxc.rightCols(3) = -R_w_c1;

    // Vector3d p_c0 = R_w_c0 * (p_w - t_c0_w);
    // Vector3d p_c1 = R_w_c1 * (p_w - t_c1_w);
    // 对地图点的雅可比
    // p_c0 对 p_w
    Matrix3d dpc0_dpg = R_w_c0;
    // p_c1 对 p_w
    Matrix3d dpc1_dpg = R_w_c1;

    // 两个雅可比
    H_x = dz_dpc0 * dpc0_dxc + dz_dpc1 * dpc1_dxc;
    H_f = dz_dpc0 * dpc0_dpg + dz_dpc1 * dpc1_dpg;

<3> 对H矩阵的可观测性约束

H矩阵就是误差状态的雅可比矩阵。

下面代码u就是对于了这个大矩阵（右），A就是对位姿的雅可比矩阵（左）。约束限制就是 $A u = 0 = w$ 。所以下面公式里的w其实是0.
$\mathbf{H}_{cam}\begin{bmatrix}\mathbf{H}_{\boldsymbol{\theta}_G}&\mathbf{H}_{\mathbf{p}_l}\end{bmatrix}\begin{bmatrix}\mathbf{C}\left(\hat{\boldsymbol{q}}_{G,k|k-1}\right)^G\mathbf{g}\\\left(\left\lfloor G\hat{\mathbf{f}}_{k|k-1}\times\right\rfloor-\left\lfloor{}^G\hat{\mathbf{p}}_{I,k|k-1}\times\right\rfloor\right)^G\mathbf{g}\end{bmatrix}=\mathbf{0}.$

$\mathbf{H}_{cam}$ 对应代码dz_dpc，即残差对相机系点 $^CP$

$\mathbf{H}_{\theta_G}$ 对应代码dpc_dxc，即相机系点 $^CP$ 对旋转的雅可比

$\mathbf{H}_{\mathbf{p}_l}$ 对应代码dpc_dxc，即相机系点 $^CP$ 对平移的雅可比

$\mathbf{H}_{\mathbf{f}}$ 对应代码dpc_dpg，即相机系点 $^CP$ 对路标点 $^WP$ 的雅可比

$\mathbf{A}^*=\mathbf{A}-(\mathbf{A}\mathbf{u}-\mathbf{w})\left(\mathbf{u}^T\mathbf{u}\right)^{-1}\mathbf{u}^T$

解决来之后，根据下面公式依次对应新的雅可比。代码中是双目，所以行维都是4.

$\mathbf{H}_{cam}\mathbf{H}_{\theta_G}=\mathbf{A}_{1:2,1:3}^{\prime},\mathbf{H}_{cam}\mathbf{H}_{\mathbf{p}_l}=\mathbf{A}_{1:2,4:6}^{\prime},\mathbf{H}_{cam}\mathbf{H}_{\mathbf{f}}=-\mathbf{A}_{1:2,4:6}^{\prime}$

    // Modifty the measurement Jacobian to ensure
    // observability constrain.
    // 6. OC
    Matrix<double, 4, 6> A = H_x;
    Matrix<double, 6, 1> u = Matrix<double, 6, 1>::Zero();
    u.block<3, 1>(0, 0) = 
        quaternionToRotation(cam_state.orientation_null) * IMUState::gravity;
    u.block<3, 1>(3, 0) =
        skewSymmetric(p_w - cam_state.position_null) * IMUState::gravity;
    H_x = A - A * u * (u.transpose() * u).inverse() * u.transpose();
    H_f = -H_x.block<4, 3>(0, 3);//4*3大小，从0行3列开始取，对应公式

<4> 计算残差r

残差 = 观测 - 预测

对于观测，就是视觉中经过匹配得到的结果。

对于预测，就是利用imu状态估计出来的cam状态进行预测的。

    // 7. 计算归一化平面坐标误差 = 观测 - 预测
    r = z - Vector4d(p_c0(0) / p_c0(2), p_c0(1) / p_c0(2),
                        p_c1(0) / p_c1(2), p_c1(1) / p_c1(2));

③ 左零空间投影

消除路标点带来的不确定性
$\mathbf{r}^{j}=\mathbf{H}_{\mathrm{x}}^{j}\tilde{\mathbf{x}}+\mathbf{H}_{f}^{j~G}\tilde{\mathbf{p}}_{j}+\mathbf{n}^{j}$
左零空间投影
$\mathbf{r}_o^j=\mathbf{V}^\top\mathbf{r}^j=\mathbf{V}^\top\mathbf{H}_\mathrm{x}^j\tilde{\mathbf{x}}+\mathbf{V}^\top\mathbf{n}^j=\mathbf{H}_{\mathrm{x},o}^j\tilde{\mathbf{x}}+\mathbf{n}_o^j$

注意，jacobian_row_size - 3，为什么是减三，因为对特征点列雅可比的维度是3。理论分析哪里用QR分解解释了，把Q分为Q1和Q2，选择右边的Q2，R是jacobian_row_size*3上三角

但是原作者代码实现用的是SVD分解，得到结果和QR分解结果一致！

    // Project the residual and Jacobians onto the nullspace
    // of H_fj.
    // 零空间投影
    JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
    MatrixXd A = svd_helper.matrixU().rightCols(
        jacobian_row_size - 3);

    // 上面的效果跟QR分解一样，下面的代码可以测试打印对比
    // Eigen::ColPivHouseholderQR<MatrixXd> qr(H_fj);
	// MatrixXd Q = qr.matrixQ();
    // std::cout << "spqr_helper.matrixQ(): " << std::endl << Q << std::endl << std::endl;
    // std::cout << "A: " << std::endl << A << std::endl;

    // 0空间投影
    H_x = A.transpose() * H_xj;
    r = A.transpose() * r_j;

    return;
}

4.5.3 状态更新

零空间投影后的H矩阵和残差r
$\mathbf{r}_o^j=\mathbf{V}^\top\mathbf{r}^j=\mathbf{V}^\top\mathbf{H}_\mathrm{x}^j\tilde{\mathbf{x}}+\mathbf{V}^\top\mathbf{n}^j=\mathbf{H}_{\mathrm{x},o}^j\tilde{\mathbf{x}}+\mathbf{n}_o^j$

   // resize成实际大小
    H_x.conservativeResize(stack_cntr, H_x.cols());
    r.conservativeResize(stack_cntr);

    // 7. 使用误差及雅可比更新状态
    measurementUpdate(H_x, r);

在理论部分中，为了降低计算量，利用QR分解的特点（行维>>列维，这部分可以参考QR分解推导线性方程那部分），对H矩阵进行降维，详见后端理论推导。

① 降维

这段代码通过QR分解，将输入矩阵 H 投影到QR分解的Q的转置上，然后提取其中的前几行，从而实现对矩阵 H 的降维。这种处理通常用于稀疏矩阵，可以减少计算量。

留待后续

/**
 * @brief 更新
 * @param  H 雅可比
 * @param  r 误差
 */
void MsckfVio::measurementUpdate(
    const MatrixXd &H, const VectorXd &r)
{

    if (H.rows() == 0 || r.rows() == 0)
        return;

    // Decompose the final Jacobian matrix to reduce computational
    // complexity as in Equation (28), (29).
    MatrixXd H_thin;
    VectorXd r_thin;

    if (H.rows() > H.cols())
    {
        // Convert H to a sparse matrix.
        SparseMatrix<double> H_sparse = H.sparseView();

        // Perform QR decompostion on H_sparse.
        // 利用H矩阵稀疏性，QR分解
        // 这段结合零空间投影一起理解，主要作用就是降低计算量
        SPQR<SparseMatrix<double>> spqr_helper;
        spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
        spqr_helper.compute(H_sparse);

        MatrixXd H_temp;
        VectorXd r_temp;
        (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
        (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);

        H_thin = H_temp.topRows(21 + state_server.cam_states.size() * 6);
        r_thin = r_temp.head(21 + state_server.cam_states.size() * 6);

        // HouseholderQR<MatrixXd> qr_helper(H);
        // MatrixXd Q = qr_helper.householderQ();
        // MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6);

        // H_thin = Q1.transpose() * H;
        // r_thin = Q1.transpose() * r;
    }
    else
    {
        H_thin = H;
        r_thin = r;
    }

② 卡尔曼滤波更新

最关键的卡尔曼滤波更新

r即计算的残差，对于代码delta_x
$\Delta\mathbf{X}=\mathbf{Kr}_n$

计算卡尔曼增益，进而计算误差状态的更新过程，V是观测方程的高斯噪声。

$\begin{aligned} &\boldsymbol{K}&& =P_\text{pred}H^\top(HP_\text{pred}H^\top+V)^{-1}, \\ &\delta x&& =K(z-h(\boldsymbol{x_\mathrm{pred}})) = Kr, \\ &\boldsymbol{x}&& =x_{\mathrm{pred}}+\delta\boldsymbol{x}, \\ &\text{P}&& =(\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H})\boldsymbol{P}_{\mathrm{pred}}. \end{aligned}$

    // 2. 标准的卡尔曼计算过程
    // Compute the Kalman gain.
    const MatrixXd &P = state_server.state_cov;
    MatrixXd S = H_thin * P * H_thin.transpose() +
                    Feature::observation_noise * MatrixXd::Identity(
                                                    H_thin.rows(), H_thin.rows());
    // MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H_thin*P);
    MatrixXd K_transpose = S.ldlt().solve(H_thin * P);
    MatrixXd K = K_transpose.transpose();

    // Compute the error of the state.
    VectorXd delta_x = K * r_thin;	// δx

    // Update the IMU state.
    const VectorXd &delta_x_imu = delta_x.head<21>();

    if ( // delta_x_imu.segment<3>(0).norm() > 0.15 ||
            // delta_x_imu.segment<3>(3).norm() > 0.15 ||
        delta_x_imu.segment<3>(6).norm() > 0.5 ||
        // delta_x_imu.segment<3>(9).norm() > 0.5 ||
        delta_x_imu.segment<3>(12).norm() > 1.0)
    {
        printf("delta velocity: %f\n", delta_x_imu.segment<3>(6).norm());
        printf("delta position: %f\n", delta_x_imu.segment<3>(12).norm());
        ROS_WARN("Update change is too large.");
        // return;
    }

    // 3. 更新到imu状态量
    const Vector4d dq_imu =
        smallAngleQuaternion(delta_x_imu.head<3>());
    // 相当于左乘dq_imu
    state_server.imu_state.orientation = quaternionMultiplication(
        dq_imu, state_server.imu_state.orientation);
    state_server.imu_state.gyro_bias += delta_x_imu.segment<3>(3);
    state_server.imu_state.velocity += delta_x_imu.segment<3>(6);
    state_server.imu_state.acc_bias += delta_x_imu.segment<3>(9);
    state_server.imu_state.position += delta_x_imu.segment<3>(12);

    // 外参
    const Vector4d dq_extrinsic =
        smallAngleQuaternion(delta_x_imu.segment<3>(15));
    state_server.imu_state.R_imu_cam0 =
        quaternionToRotation(dq_extrinsic) * state_server.imu_state.R_imu_cam0;
    state_server.imu_state.t_cam0_imu += delta_x_imu.segment<3>(18);

    // Update the camera states.
    // 更新相机姿态
    auto cam_state_iter = state_server.cam_states.begin();
    for (int i = 0; i < state_server.cam_states.size(); ++i, ++cam_state_iter)
    {
        const VectorXd &delta_x_cam = delta_x.segment<6>(21 + i * 6);
        const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
        cam_state_iter->second.orientation = quaternionMultiplication(
            dq_cam, cam_state_iter->second.orientation);
        cam_state_iter->second.position += delta_x_cam.tail<3>();
    }

    // Update state covariance.
    // 4. 更新协方差
    MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K * H_thin;
    // state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
    //   K*K.transpose()*Feature::observation_noise;
    state_server.state_cov = I_KH * state_server.state_cov;

    // Fix the covariance to be symmetric
    MatrixXd state_cov_fixed = (state_server.state_cov +
                                state_server.state_cov.transpose()) /
                                2.0;
    state_server.state_cov = state_cov_fixed;

    return;
}

4.6 pruneCamStateBuffer

/**
 * @brief 当cam状态数达到最大值时，挑出若干cam状态待删除
 */
void MsckfVio::pruneCamStateBuffer()
{
    // 数量还不到该删的程度，配置文件里面是20个
    if (state_server.cam_states.size() < max_cam_state_size)
        return;

    // Find two camera states to be removed.
    // 1. 找出该删的相机状态的id，两个
    vector<StateIDType> rm_cam_state_ids(0);
    findRedundantCamStates(rm_cam_state_ids);

    // Find the size of the Jacobian matrix.
    // 2. 找到删减帧涉及的观测雅可比大小
    int jacobian_row_size = 0;
    for (auto &item : map_server)
    {
        auto &feature = item.second;
        // Check how many camera states to be removed are associated
        // with this feature.
        // 2.1 在待删去的帧中统计能观测到这个特征的帧
        vector<StateIDType> involved_cam_state_ids(0);
        for (const auto &cam_id : rm_cam_state_ids)
        {
            if (feature.observations.find(cam_id) !=
                feature.observations.end())
                involved_cam_state_ids.push_back(cam_id);
        }

        if (involved_cam_state_ids.size() == 0)
            continue;
        // 2.2 这个点只在一个里面有观测那就直接删
        // 只用一个观测更新不了状态
        if (involved_cam_state_ids.size() == 1)
        {
            feature.observations.erase(involved_cam_state_ids[0]);
            continue;
        }
        // 程序到这里的时候说明找到了一个特征，先不说他一共被几帧观测到
        // 到这里说明被两帧或两帧以上待删减的帧观测到
        // 2.3 如果没有做过三角化，做一下三角化，如果失败直接删
        if (!feature.is_initialized)
        {
            // Check if the feature can be initialize.
            if (!feature.checkMotion(state_server.cam_states))
            {
                // If the feature cannot be initialized, just remove
                // the observations associated with the camera states
                // to be removed.
                for (const auto &cam_id : involved_cam_state_ids)
                    feature.observations.erase(cam_id);
                continue;
            }
            else
            {
                if (!feature.initializePosition(state_server.cam_states))
                {
                    for (const auto &cam_id : involved_cam_state_ids)
                        feature.observations.erase(cam_id);
                    continue;
                }
            }
        }

        // 2.4 最后的最后得出了行数
        // 意味着有involved_cam_state_ids.size() 数量的观测要被删去
        // 但是因为待删去的帧间有共同观测的关系，直接删会损失这部分信息
        // 所以临删前做最后一次更新
        jacobian_row_size += 4 * involved_cam_state_ids.size() - 3;
    }

    // cout << "jacobian row #: " << jacobian_row_size << endl;

    // Compute the Jacobian and residual.
    // 3. 计算待删掉的这部分观测的雅可比与误差
    // 预设大小
    MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
                                    21 + 6 * state_server.cam_states.size());
    VectorXd r = VectorXd::Zero(jacobian_row_size);
    int stack_cntr = 0;

    // 又做了一遍类似上面的遍历，只不过该三角化的已经三角化，该删的已经删了
    for (auto &item : map_server)
    {
        auto &feature = item.second;
        // Check how many camera states to be removed are associated
        // with this feature.
        // 这段就是判断一下这个点是否都在待删除帧中有观测
        vector<StateIDType> involved_cam_state_ids(0);
        for (const auto &cam_id : rm_cam_state_ids)
        {
            if (feature.observations.find(cam_id) !=
                feature.observations.end())
                involved_cam_state_ids.push_back(cam_id);
        }

        // 一个的情况已经被删掉了
        if (involved_cam_state_ids.size() == 0)
            continue;

        // 计算出待删去的这部分的雅可比
        // 这个点假如有多个观测，但本次只用待删除帧上的观测
        MatrixXd H_xj;
        VectorXd r_j;
        featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);

        if (gatingTest(H_xj, r_j, involved_cam_state_ids.size()))
        {
            H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
            r.segment(stack_cntr, r_j.rows()) = r_j;
            stack_cntr += H_xj.rows();
        }

        // 删去观测
        for (const auto &cam_id : involved_cam_state_ids)
            feature.observations.erase(cam_id);
    }

    H_x.conservativeResize(stack_cntr, H_x.cols());
    r.conservativeResize(stack_cntr);

    // Perform measurement update.
    // 4. 用待删去的这些观测更新一下
    measurementUpdate(H_x, r);

    // 5. 直接删掉对应的行列，直接干掉
    // 为啥没有做类似于边缘化的操作？
    // 个人认为是上面做最后的更新了，所以信息已经更新到了各个地方
    for (const auto &cam_id : rm_cam_state_ids)
    {
        int cam_sequence = std::distance(
            state_server.cam_states.begin(), state_server.cam_states.find(cam_id));
        int cam_state_start = 21 + 6 * cam_sequence;
        int cam_state_end = cam_state_start + 6;

        // Remove the corresponding rows and columns in the state
        // covariance matrix.
        if (cam_state_end < state_server.state_cov.rows())
        {
            state_server.state_cov.block(cam_state_start, 0,
                                            state_server.state_cov.rows() - cam_state_end,
                                            state_server.state_cov.cols()) =
                state_server.state_cov.block(cam_state_end, 0,
                                                state_server.state_cov.rows() - cam_state_end,
                                                state_server.state_cov.cols());

            state_server.state_cov.block(0, cam_state_start,
                                            state_server.state_cov.rows(),
                                            state_server.state_cov.cols() - cam_state_end) =
                state_server.state_cov.block(0, cam_state_end,
                                                state_server.state_cov.rows(),
                                                state_server.state_cov.cols() - cam_state_end);

            state_server.state_cov.conservativeResize(
                state_server.state_cov.rows() - 6, state_server.state_cov.cols() - 6);
        }
        else
        {
            state_server.state_cov.conservativeResize(
                state_server.state_cov.rows() - 6, state_server.state_cov.cols() - 6);
        }

        // Remove this camera state in the state vector.
        state_server.cam_states.erase(cam_id);
    }

    return;
}

4.7 发布位姿publish

void MsckfVio::publish(const ros::Time &time)
{

    // Convert the IMU frame to the body frame.
    // 1. 计算body坐标，因为imu与body相对位姿是单位矩阵，所以就是imu的坐标
    const IMUState &imu_state = state_server.imu_state;
    Eigen::Isometry3d T_i_w = Eigen::Isometry3d::Identity();
    T_i_w.linear() = quaternionToRotation(
                            imu_state.orientation)
                            .transpose();
    T_i_w.translation() = imu_state.position;

    Eigen::Isometry3d T_b_w = IMUState::T_imu_body * T_i_w *
                                IMUState::T_imu_body.inverse();
    Eigen::Vector3d body_velocity =
        IMUState::T_imu_body.linear() * imu_state.velocity;

    // Publish tf
    // 2. 发布tf，实时的位姿，没有轨迹，没有协方差
    if (publish_tf)
    {
        tf::Transform T_b_w_tf;
        tf::transformEigenToTF(T_b_w, T_b_w_tf);
        tf_pub.sendTransform(tf::StampedTransform(
            T_b_w_tf, time, fixed_frame_id, child_frame_id));
    }

    // Publish the odometry
    // 3. 发布位姿，能在rviz留下轨迹的
    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = time;
    odom_msg.header.frame_id = fixed_frame_id;
    odom_msg.child_frame_id = child_frame_id;

    tf::poseEigenToMsg(T_b_w, odom_msg.pose.pose);
    tf::vectorEigenToMsg(body_velocity, odom_msg.twist.twist.linear);

    // Convert the covariance.
    // 协方差，取出旋转平移部分，以及它们之间的公共部分组成6自由度的协方差
    Matrix3d P_oo = state_server.state_cov.block<3, 3>(0, 0);
    Matrix3d P_op = state_server.state_cov.block<3, 3>(0, 12);
    Matrix3d P_po = state_server.state_cov.block<3, 3>(12, 0);
    Matrix3d P_pp = state_server.state_cov.block<3, 3>(12, 12);
    Matrix<double, 6, 6> P_imu_pose = Matrix<double, 6, 6>::Zero();
    P_imu_pose << P_pp, P_po, P_op, P_oo;

    // 转下坐标，但是这里都是单位矩阵
    Matrix<double, 6, 6> H_pose = Matrix<double, 6, 6>::Zero();
    H_pose.block<3, 3>(0, 0) = IMUState::T_imu_body.linear();
    H_pose.block<3, 3>(3, 3) = IMUState::T_imu_body.linear();
    Matrix<double, 6, 6> P_body_pose = H_pose *
                                        P_imu_pose * H_pose.transpose();

    // 填充协方差
    for (int i = 0; i < 6; ++i)
        for (int j = 0; j < 6; ++j)
            odom_msg.pose.covariance[6 * i + j] = P_body_pose(i, j);

    // Construct the covariance for the velocity.
    // 速度协方差
    Matrix3d P_imu_vel = state_server.state_cov.block<3, 3>(6, 6);
    Matrix3d H_vel = IMUState::T_imu_body.linear();
    Matrix3d P_body_vel = H_vel * P_imu_vel * H_vel.transpose();
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            odom_msg.twist.covariance[i * 6 + j] = P_body_vel(i, j);

    // 发布位姿
    odom_pub.publish(odom_msg);

    // Publish the 3D positions of the features that
    // has been initialized.
    // 4. 发布点云
    boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> feature_msg_ptr(
        new pcl::PointCloud<pcl::PointXYZ>());
    feature_msg_ptr->header.frame_id = fixed_frame_id;
    feature_msg_ptr->height = 1;
    for (const auto &item : map_server)
    {
        const auto &feature = item.second;
        if (feature.is_initialized)
        {
            Vector3d feature_position =
                IMUState::T_imu_body.linear() * feature.position;
            feature_msg_ptr->points.push_back(pcl::PointXYZ(
                feature_position(0), feature_position(1), feature_position(2)));
        }
    }
    feature_msg_ptr->width = feature_msg_ptr->points.size();

    feature_pub.publish(feature_msg_ptr);

    return;
}

4.8 是否重置系统onlineReset

void MsckfVio::onlineReset()
{

    // Never perform online reset if position std threshold
    // is non-positive.
    if (position_std_threshold <= 0)
        return;
    static long long int online_reset_counter = 0;

    // Check the uncertainty of positions to determine if
    // the system can be reset.
    double position_x_std = std::sqrt(state_server.state_cov(12, 12));
    double position_y_std = std::sqrt(state_server.state_cov(13, 13));
    double position_z_std = std::sqrt(state_server.state_cov(14, 14));

    if (position_x_std < position_std_threshold &&
        position_y_std < position_std_threshold &&
        position_z_std < position_std_threshold)
        return;

    ROS_WARN("Start %lld online reset procedure...",
                ++online_reset_counter);
    ROS_INFO("Stardard deviation in xyz: %f, %f, %f",
                position_x_std, position_y_std, position_z_std);

    // Remove all existing camera states.
    state_server.cam_states.clear();

    // Clear all exsiting features in the map.
    map_server.clear();

    // Reset the state covariance.
    double gyro_bias_cov, acc_bias_cov, velocity_cov;
    nh.param<double>("initial_covariance/velocity",
                        velocity_cov, 0.25);
    nh.param<double>("initial_covariance/gyro_bias",
                        gyro_bias_cov, 1e-4);
    nh.param<double>("initial_covariance/acc_bias",
                        acc_bias_cov, 1e-2);

    double extrinsic_rotation_cov, extrinsic_translation_cov;
    nh.param<double>("initial_covariance/extrinsic_rotation_cov",
                        extrinsic_rotation_cov, 3.0462e-4);
    nh.param<double>("initial_covariance/extrinsic_translation_cov",
                        extrinsic_translation_cov, 1e-4);

    state_server.state_cov = MatrixXd::Zero(21, 21);
    for (int i = 3; i < 6; ++i)
        state_server.state_cov(i, i) = gyro_bias_cov;
    for (int i = 6; i < 9; ++i)
        state_server.state_cov(i, i) = velocity_cov;
    for (int i = 9; i < 12; ++i)
        state_server.state_cov(i, i) = acc_bias_cov;
    for (int i = 15; i < 18; ++i)
        state_server.state_cov(i, i) = extrinsic_rotation_cov;
    for (int i = 18; i < 21; ++i)
        state_server.state_cov(i, i) = extrinsic_translation_cov;

    ROS_WARN("%lld online reset complete...", online_reset_counter);
    return;
}