S-MSCKF 学习记录

featureCallback

if (!is_gravity_set) return; 

判断imu是否初始化

1. imu数据预积分

batchImuProcessing(msg->header.stamp.toSec());

循环遍历imu_msg_buffer把state_server.imu_state.time到msg->header.stamp.toSec()中间的imu数据输入void MsckfVio::processModel(const double& time, const Vector3d& m_gyro, const Vector3d& m_acc)中。

  IMUState& imu_state = state_server.imu_state;
  Vector3d gyro = m_gyro - imu_state.gyro_bias;
  Vector3d acc = m_acc - imu_state.acc_bias;
  double dtime = time - imu_state.time;

消除bias对数据的影响

  Matrix<double, 21, 21> F = Matrix<double, 21, 21>::Zero();
  Matrix<double, 21, 12> G = Matrix<double, 21, 12>::Zero();

  F.block<3, 3>(0, 0) = -skewSymmetric(gyro);
  F.block<3, 3>(0, 3) = -Matrix3d::Identity();
  F.block<3, 3>(6, 0) = -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();
  
  Matrix<double, 21, 21> Fdt = F * dtime;
  Matrix<double, 21, 21> Fdt_square = Fdt * Fdt;
  Matrix<double, 21, 21> Fdt_cube = Fdt_square * Fdt;
  Matrix<double, 21, 21> Phi = Matrix<double, 21, 21>::Identity() +
    Fdt + 0.5*Fdt_square + (1.0/6.0)*Fdt_cube;

更新F和G矩阵
连续形式下误差状态的运动方程为： X̃̇_IMU = F^15×15 X̃ _IMU + G ^15×12 n _IMU ^12×1
X̃ _IMU 为上一时刻的预积分量，n _IMU ^12×1这一时刻的数据
离散形式下的运动方程为： X̃_IMUk+1 = Φ(t_k + ∆T, t_k)X̃_IMUk + (G∆T)n_IMU
近似形式: Φ(t_k + ∆T, t_k) = exp(F∆T) ≅ I^15×15 + F∆T
代码中使用 Φ ≈ I^15*15 + Fdt + 1/2 * (Fdt)² + 1/6 * (Fdt)³

预测imu状态

void MsckfVio::predictNewState(const double& dt, const Vector3d& gyro, const Vector3d& acc)

利用 RK4 积分预测状态

更新Q

参考q_k+1 = q ⊗ q_k

dq_dt = (cos(gyro_norm * dt * 0.5) * Matrix4d::Identity() + 1 / gyro_norm * sin(gyro_norm * dt * 0.5) * Omega) * q;

下面使用RK4积分更新p和v

Matrix3d dR_dt_transpose = quaternionToRotation(dq_dt).transpose();
 Matrix3d dR_dt2_transpose = quaternionToRotation(dq_dt2).transpose();

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

 // k2 = f(tn+dt/2, yn+k1*dt/2)
 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)
 q = dq_dt;
 quaternionNormalize(q);
 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);

更新转移矩阵Φ

更新协方差

更新II的协方差

// Propogate the state covariance matrix.
 Matrix<double, 21, 21> Q = Phi * G * state_server.continuous_noise_cov * G.transpose() * Phi.transpose() * dtime;

Q_k ≈ Φ_kGQGΦ^T_kΔt

  state_server.state_cov.block<21, 21>(0, 0) = Phi*state_server.state_cov.block<21, 21>(0, 0)*Phi.transpose() + Q;

P_IIk+1∣k = Φ_kP_IIk∣kΦ_k^T + Q_k

更新IC的协方差

P _ICk+1|k = Φ * P _ICk1|k
P _CIk+1|k = P _CIk1|k * Φ^T

  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;

删除积分过的imu数据

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

删除已经更新过的状态

2.状态更新 void MsckfVio::stateAugmentation(const double& time)

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

计算世界坐标系到camera坐标系的转换

 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;

初始化和imu数据对应的camera_state