MSCKF中的IMU参数标定

在MSCKF中，IMU的参数可以在线标定。在初始阶段，采集IMU不动时的200帧信号，平均得到的角速度为IMU角速度的偏置，平均线速度的大小为重力的大小，并求出平均线速度与世界坐标系的旋转。
代码如下：

void MsckfVio::initializeGravityAndBias() {

  // Initialize gravity and gyro bias.
  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(imu_msg.angular_velocity, angular_vel);
    tf::vectorMsgToEigen(imu_msg.linear_acceleration, linear_acc);

    sum_angular_vel += angular_vel;
    sum_linear_acc += linear_acc;
  }

  state_server.imu_state.gyro_bias =
    sum_angular_vel / imu_msg_buffer.size();//方向的偏置
  //IMUState::gravity =
  //  -sum_linear_acc / imu_msg_buffer.size();
  // This is the gravity in the IMU frame.
  Vector3d gravity_imu =
    sum_linear_acc / imu_msg_buffer.size(); //重力方向

  // Initialize the initial orientation, so that the estimation
  // is consistent with the inertial frame.
  double gravity_norm = gravity_imu.norm();
  IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);

  Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
    gravity_imu, -IMUState::gravity);
  state_server.imu_state.orientation =
    rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());//当前重力方向到世界坐标系的旋转

  return;
}

参考：https://github.com/KumarRobotics/msckf_vio