MSCKF-vio源码阅读

作为一个菜狗来说，一开始弄明白kf、ekf等滤波方法实属不易，但是一旦理解原理之后再发散到基于滤波的状态估计方法，学习起来就会事半功倍，就像导航包中的robot_pose_ekf，以及早年间一众基于ekf 的slam方法。当然，基于EKF的SLAM方法，采取的是递归的估计权重增益与协方差矩阵，需要更新的状态量包括相机的位姿以及所有路标点，在建图过程中路标点会逐渐增多导致状态转移矩阵变得很大，出现深度学习中常见的“维度爆炸”问题，EKF无法继续运行，因此EKF-SLAM不能在大场景下使用。

vio大致有两种，中心思想分别是基于优化和基于滤波，第一种的代表是更为熟知的Vins-MONO和okvis，第二种就是msckf，msckf全称是multi status constrainted kf，也就是多状态约束的ekf滤波，它也是一种EKF的SLAM方法，但是采用了滑动窗口法对相机位姿进行数量控制，其约束包括imu的积分，以及不同相机位姿下共同观看到的路标点。这样它弥补了维度爆炸的问题，在较大场景中也能游刃有余地进行状态估计。

在我阅读的这篇代码中，它采取的是双目视觉与imu的融合，双目使用LK光流法对特征进行跟踪，它由两个节点构成，分别是特征提取与状态估计部分。

一、图像特征的跟踪与重构

第一个节点是图像特征的提取。图像特征的采样很简单，是采取fast特征进行跟踪，LK光流法是一种半直接法估计相机位姿的方法，利用的是灰度不变的假设。

void ImageProcessor::stereoCallback(
    const sensor_msgs::ImageConstPtr& cam0_img,
    const sensor_msgs::ImageConstPtr& cam1_img) {

  //cout << "==================================" << endl;

  // Get the current image.
  cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
      sensor_msgs::image_encodings::MONO8);
  cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
      sensor_msgs::image_encodings::MONO8);

  // Build the image pyramids once since they're used at multiple places
  //分别建立左右两个图像的金字塔模型，金字塔的目的是获取不同尺度的特征模型，在较快速运动的情况下仍能获取有效的像素变化趋势
  createImagePyramids();

  // Detect features in the first frame.
  if (is_first_img) {
    ros::Time start_time = ros::Time::now();
    //如果是第一帧，则采样左右图像的特征点，并匹配左右特征点以便三角化
    initializeFirstFrame();
    //ROS_INFO("Detection time: %f",
    //    (ros::Time::now()-start_time).toSec());
    is_first_img = false;

    // Draw results.
    start_time = ros::Time::now();
    drawFeaturesStereo();
    //ROS_INFO("Draw features: %f",
    //    (ros::Time::now()-start_time).toSec());
  } else {
    // Track the feature in the previous image.
    ros::Time start_time = ros::Time::now();
    //如果是中途帧，则对前面几帧进行跟踪
    trackFeatures();
    //ROS_INFO("Tracking time: %f",
    //    (ros::Time::now()-start_time).toSec());

    // Add new features into the current image.
    start_time = ros::Time::now();
    addNewFeatures();
    //ROS_INFO("Addition time: %f",
    //    (ros::Time::now()-start_time).toSec());

    // Add new features into the current image.
    start_time = ros::Time::now();
    pruneGridFeatures();
    //ROS_INFO("Prune grid features: %f",
    //    (ros::Time::now()-start_time).toSec());

    // Draw results.
    start_time = ros::Time::now();
    drawFeaturesStereo();
    //ROS_INFO("Draw features: %f",
    //    (ros::Time::now()-start_time).toSec());
  }

  //ros::Time start_time = ros::Time::now();
  //updateFeatureLifetime();
  //ROS_INFO("Statistics: %f",
  //    (ros::Time::now()-start_time).toSec());

  // Publish features in the current image.
  ros::Time start_time = ros::Time::now();
  publish();
  //ROS_INFO("Publishing: %f",
  //    (ros::Time::now()-start_time).toSec());

  // Update the previous image and previous features.
  cam0_prev_img_ptr = cam0_curr_img_ptr;
  prev_features_ptr = curr_features_ptr;
  std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);

  // Initialize the current features to empty vectors.
  curr_features_ptr.reset(new GridFeatures());
  for (int code = 0; code <
      processor_config.grid_row*processor_config.grid_col; ++code) {
    (*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
  }

  return;
}

那么大致的双目跟踪的实现为：

1.初始化第一帧时的左右两帧跟踪，用左帧的特征点来对右帧的特征点进行匹配，删除不符合双目位姿变换的点。

 2.在跟踪过程中，需要上一帧的先验，当然，我们将对两帧之间的imu数据进行积分，作为先验提供给相机特征的位姿估计。在前后两帧的跟踪中，用calcOpticalFlowPyrLK进行特征点跟踪。

void ImageProcessor::integrateImuData(
    Matx33f& cam0_R_p_c, Matx33f& cam1_R_p_c) {
  // Find the start and the end limit within the imu msg buffer.
  //迭代器城会玩系列～获取这两帧之间的imu数据
  auto begin_iter = imu_msg_buffer.begin();
  while (begin_iter != imu_msg_buffer.end()) {
    if ((begin_iter->header.stamp-
          cam0_prev_img_ptr->header.stamp).toSec() < -0.01)
      ++begin_iter;
    else
      break;
  }

  auto end_iter = begin_iter;
  while (end_iter != imu_msg_buffer.end()) {
    if ((end_iter->header.stamp-
          cam0_curr_img_ptr->header.stamp).toSec() < 0.005)
      ++end_iter;
    else
      break;
  }

  // Compute the mean angular velocity in the IMU frame.
  //这里应该是把imu视为匀速运动，计算得到imu平均的角速度
  Vec3f mean_ang_vel(0.0, 0.0, 0.0);
  for (auto iter = begin_iter; iter < end_iter; ++iter)
    mean_ang_vel += Vec3f(iter->angular_velocity.x,
        iter->angular_velocity.y, iter->angular_velocity.z);

  if (end_iter-begin_iter > 0)
    mean_ang_vel *= 1.0f / (end_iter-begin_iter);

  // Transform the mean angular velocity from the IMU
  // frame to the cam0 and cam1 frames.
  //在算法开始前我们标定了两个相机与imu的外参，即R_cam0_imu和R_cam1_imu，这样就计算得到相机的角速度
  Vec3f cam0_mean_ang_vel = R_cam0_imu.t() * mean_ang_vel;
  Vec3f cam1_mean_ang_vel = R_cam1_imu.t() * mean_ang_vel;

  // Compute the relative rotation.
  //视为匀速运动之后就用罗德里格斯公式将旋转向量转化为旋转矩阵，这里是3*3的旋转矩阵
  double dtime = (cam0_curr_img_ptr->header.stamp-
      cam0_prev_img_ptr->header.stamp).toSec();
  Rodrigues(cam0_mean_ang_vel*dtime, cam0_R_p_c);
  Rodrigues(cam1_mean_ang_vel*dtime, cam1_R_p_c);
  cam0_R_p_c = cam0_R_p_c.t();
  cam1_R_p_c = cam1_R_p_c.t();

  // Delete the useless and used imu messages.
  //清空本次imu buffer
  imu_msg_buffer.erase(imu_msg_buffer.begin(), end_iter);
  return;
}

因此，在第一个节点部分，我们得到了双目相机提取到的特征点，并且对相机位姿进行了初步的先验，下一个节点是对相机位姿、imu状态进行统一的更新。

二、EKF的约束与更新

对于vio来说数据融合与约束的功夫要远大于前端帧到帧的视觉匹配。msckf-vio的第二部分是基于imu数据的状态更新，或者说，是基于相机位姿、图像特征点、imu状态量的多重状态的卡尔曼滤波更新。这个节点接收的话题是上个节点传送的特征点以及imu传感器信息，最终相机的odometry与tf变换也是callback函数所驱动的。

1、imu的预处理

首先是imu的callback函数，它只做一件事，就是在前200帧校正重力与bias，也就是说前200帧需要保持静止以确定重力方向以及bg（重力的bias）。

第二个callback函数是来自第一个节点的特征点，在这里进行了所有的数据融合处理，在预处理部分是batchImuProcessing函数，它将两帧之间的imu抽取并在processModel函数中预积分。

void MsckfVio::processModel(const double& time,
    const Vector3d& m_gyro,
    const Vector3d& m_acc) {

  // Remove the bias from the measured gyro and acceleration
  IMUState& imu_state = state_server.imu_state;
  //测量值减去bias
  Vector3d gyro = m_gyro - imu_state.gyro_bias;//角速度的“真值”
  Vector3d acc = m_acc - imu_state.acc_bias;//加速度的“真值”
  double dtime = time - imu_state.time;

  // Compute discrete transition and noise covariance matrix
  //F矩阵是imu各向量的误差状态转移矩阵，G矩阵是imu测量噪声与bias的转移矩阵
  //F中的零矩阵块代表着某些状态之间无关
  Matrix<double, 21, 21> F = Matrix<double, 21, 21>::Zero();
  Matrix<double, 21, 12> G = Matrix<double, 21, 12>::Zero();
  //开始往里填数据了，状态向量都是误差值！因此更新的是误差值的导数！
  //姿态角对姿态角的转移矩阵，skewSymmetric是反对称矩阵
  F.block<3, 3>(0, 0) = -skewSymmetric(gyro);
  //姿态角bias对姿态角的转移矩阵
  F.block<3, 3>(0, 3) = -Matrix3d::Identity();
  //姿态角对角速度的转移矩阵
  F.block<3, 3>(6, 0) = -quaternionToRotation(
      imu_state.orientation).transpose()*skewSymmetric(acc);
  //加速度bias对角速度的转移矩阵
  F.block<3, 3>(6, 9) = -quaternionToRotation(
      imu_state.orientation).transpose();
  //加速度对位置的转移矩阵
  F.block<3, 3>(12, 6) = Matrix3d::Identity();
  
  //噪声与bias对imu状态向量变化的影响
  //姿态角噪声对姿态角
  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.
  Matrix<double, 21, 21> Fdt = F * dtime;
  Matrix<double, 21, 21> Fdt_square = Fdt * Fdt;
  Matrix<double, 21, 21> Fdt_cube = Fdt_square * Fdt;
  //根据之前的状态转移方程，利用解微分方程的方法，可以将imu误差状态量解为exp（Fdt），再进行线性化泰勒展开
  Matrix<double, 21, 21> Phi = Matrix<double, 21, 21>::Identity() +
    Fdt + 0.5*Fdt_square + (1.0/6.0)*Fdt_cube;

  // Propogate the state using 4th order Runge-Kutta
  //用四阶龙格库塔积分进行pvq的预积分
  predictNewState(dtime, gyro, acc);

  // Modify the transition matrix
  Matrix3d R_kk_1 = quaternionToRotation(imu_state.orientation_null);
  Phi.block<3, 3>(0, 0) =
    quaternionToRotation(imu_state.orientation) * R_kk_1.transpose();

  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;

  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;

  // Propogate the state covariance matrix.
  //根据状态量变换矩阵求协方差的预测值
  Matrix<double, 21, 21> Q = Phi*G*state_server.continuous_noise_cov*
    G.transpose()*Phi.transpose()*dtime;
  state_server.state_cov.block<21, 21>(0, 0) =
    Phi*state_server.state_cov.block<21, 21>(0, 0)*Phi.transpose() + Q;

  if (state_server.cam_states.size() > 0) {
    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();
  }

  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.
  imu_state.orientation_null = imu_state.orientation;
  imu_state.position_null = imu_state.position;
  imu_state.velocity_null = imu_state.velocity;

  // Update the state info
  state_server.imu_state.time = time;
  return;
}

2、状态扩增

在msckf中，我们采用的是eskf的方法，也就是约束和更新的是状态向量的残差，这么做的原因是残差的值靠近原点，线性化的误差较小。与vins-mono相同，其状态向量为imu的p、v、q、ba、bg，以及相机位姿的p、q，但优化的都是它们的误差。每当一帧图像到来，imu就用预积分的形式给出状态预测，再利用特征点的约束获取相机位姿，再将本帧相机位姿加入状态向量中，并更新它与之前状态的协方差矩阵，这称为状态扩增。

void MsckfVio::stateAugmentation(const double& time) {
  //相机imu外参
  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.
  //imu的预测位姿转到相机的预测位姿
  Matrix3d R_w_i = quaternionToRotation(
      state_server.imu_state.orientation);
  Matrix3d R_w_c = R_i_c * R_w_i;
  Vector3d t_c_w = state_server.imu_state.position +
    R_w_i.transpose()*t_c_i;
  //将该相机的状态加入到状态向量当中
  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_state.time = time;
  cam_state.orientation = rotationToQuaternion(R_w_c);
  cam_state.position = t_c_w;

  cam_state.orientation_null = cam_state.orientation;
  cam_state.position_null = cam_state.position;

  // Update the covariance matrix of the state.
  // To simplify computation, the matrix J below is the nontrivial block
  // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision
  // -aided Inertial Navigation".
  //在状态扩充时需要新加入的相机对之前imu状态的雅克比矩阵，并重新计算协方差矩阵
  Matrix<double, 6, 21> J = Matrix<double, 6, 21>::Zero();
  //在雅可比矩阵中，包括相机姿态角对imu状态的偏导数，相机位置对imu状态的偏导数，以及姿态角和位置互相的偏导数（零矩阵块）
  //相机姿态角对imu的偏导数就是外参的旋转
  J.block<3, 3>(0, 0) = R_i_c;
  J.block<3, 3>(0, 15) = Matrix3d::Identity();
  //相机位置对imu的偏导数是外参的平移转到imu坐标系下的反对称矩阵
  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);
  J.block<3, 3>(3, 12) = Matrix3d::Identity();
  J.block<3, 3>(3, 18) = Matrix3d::Identity();

  // 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.
  //P11是之前的协方差矩阵
  const Matrix<double, 21, 21>& P11 =
    state_server.state_cov.block<21, 21>(0, 0);
  //matrix.block(i,j, p, q)表示返回从矩阵(i, j)开始,每行取p个元素,每列取q个元素所组成的临时新矩阵对象,原矩阵的元素不变
  const MatrixXd& P12 =
    state_server.state_cov.block(0, 21, 21, old_cols-21);

  // Fill in the augmented state covariance.
  //扩充协方差矩阵（Pii）->（Pii,Pic;Pci,Pcc）
  state_server.state_cov.block(old_rows, 0, 6, old_cols) << J*P11, J*P12;
  state_server.state_cov.block(0, old_cols, old_rows, 6) =
    state_server.state_cov.block(old_rows, 0, 6, old_cols).transpose();
  state_server.state_cov.block<6, 6>(old_rows, old_cols) =
    J * P11 * J.transpose();

  // 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;
}

对特征点的观测残差是另一半需要关注的残差。对于一系列特征点来说，需要足够大的视差才进入计算，同时在特征点跟丢的时候才将该点纳入计算，这一段在removeLostFeatures中体现。重点在于求重投影误差对状态向量的转移矩阵。featureJacobian就是在针对某个特征点求对每个相机位姿的雅克比矩阵。

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.
  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;
  jacobian_row_size = 4 * valid_cam_state_ids.size();

  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;
  //对每个能观测到该特征点的相机帧求雅克比，并合并
  for (const auto& cam_id : valid_cam_state_ids) {
    //残差对状态向量和特征点的偏导数矩阵
    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();
    //计算单个帧的雅克比
    measurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i);

    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;
  }

  // Project the residual and Jacobians onto the nullspace
  // of H_fj.
  //fj（特征点）其实不是我们想要更新的状态量，我们用一个方法将其消去，只求残差对状态量的雅克比
  JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
  MatrixXd A = svd_helper.matrixU().rightCols(
      jacobian_row_size - 3);

  H_x = A.transpose() * H_xj;
  r = A.transpose() * r_j;

  return;
}

 在上面，包括了计算每个特征点对某个相机位姿的雅克比矩阵：

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) {

  // Prepare all the required data.
  const CAMState& cam_state = state_server.cam_states[cam_state_id];
  const Feature& feature = map_server[feature_id];

  // Cam0 pose.
  Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
  const Vector3d& t_c0_w = cam_state.position;

  // Cam1 pose.
  //求解
  Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
  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();

  // 3d feature position in the world frame.
  // And its observation with the stereo cameras.
  const Vector3d& p_w = feature.position;
  //z是该特征点在本帧的观测值
  const Vector4d& z = feature.observations.find(cam_state_id)->second;

  // 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);

  // Compute the Jacobians.
  //在这里需要分别求残差对状态量的偏导数，与残差对特征点的偏导数
  //需要链式求导，也就是z到三维点的偏导数乘上三维点到状态量的偏导数，这一点在ba优化时也是一样的
  //按理说式子中有内参，但这里应该已经转换到归一化平面上了
  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));
  //根据链式求导，接下来是计算三维坐标到状态量的偏导数
  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;

  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;
  //求对特征点的偏导数
  Matrix3d dpc0_dpg = R_w_c0;
  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;

  // Modifty the measurement Jacobian to ensure
  // observability constrain.
  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);

  // Compute the residual.
  //残差被定义为：特征点的观测值与三维点的投影值的差
  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));

  return;
}

在得到单个特征点对所有帧的状态转移矩阵后，再将所有特征点一一合并，也就是removeLostFeatures中的H_x矩阵，是我们最终关心的对状态向量的雅可比矩阵。

3、观测更新

观测更新这部分基本上是在套用EKF更新的公式，在此更新了误差状态量，以及观测减去误差的最优估计量。

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).
  //用QR分解减少运算
  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.
    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;
  }

  // Compute the Kalman gain.
  //计算卡尔曼增益
  //原有的协方差矩阵
  const MatrixXd& P = state_server.state_cov;
  //更新k-1->k的预测协方差矩阵
  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;

  // 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;
  }

  const Vector4d dq_imu =
    smallAngleQuaternion(delta_x_imu.head<3>());
  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.
  //继续套用EKF公式，从预计的协方差矩阵到更新后的协方差矩阵
  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;
}

最后做一个简单的总结，msckf的流程如下：

1.依据一段时间内imu数据进行积分，获取状态空间中imu数据的先验与imu的状态转移矩阵。

2.获得新的图像帧之后，估计本帧位姿，将本帧位姿加入状态空间中并扩充协方差矩阵。

3.计算得到特征点的三维坐标，并根据可观察到特征点进行相机位姿的约束。该约束作为观测量，更新卡尔曼增益和协方差矩阵。

4.删除超出数量或特征点不可见的相机位姿，维护一个滑动窗口的固定数量。

因此msckf中，状态空间包括了imu角速度与线速度、固定数量内的相机位姿，它们的误差都是需要预测、观测、更新的状态。而在这段窗口的路标点，我们在估计时尽量将它的值去除。