Method propagation

okvis_ros/okvis/okvis_ceres/src/ImuError.cpp:266–476

Propagates pose, speeds and biases with given IMU measurements.

// Propagates pose, speeds and biases with given IMU measurements.
int ImuError::propagation(const okvis::ImuMeasurementDeque& imuMeasurements,
                          const okvis::ImuParameters& imuParams,
                          okvis::kinematics::Transformation& T_WS,
                          okvis::SpeedAndBias& speedAndBiases,
                          const okvis::Time& t_start,
                          const okvis::Time& t_end,
                          covariance_t* covariance,
                          jacobian_t* jacobian) {
  // now the propagation
  okvis::Time time = t_start;
  okvis::Time end = t_end;

  // sanity check:
  assert(imuMeasurements.front().timeStamp <= time);
  if (!(imuMeasurements.back().timeStamp >= end)) return -1;  // nothing to do...

  // initial condition
  Eigen::Vector3d r_0 = T_WS.r();
  Eigen::Quaterniond q_WS_0 = T_WS.q();
  Eigen::Matrix3d C_WS_0 = T_WS.C();

  // increments (initialise with identity)
  Eigen::Quaterniond Delta_q(1, 0, 0, 0);
  Eigen::Matrix3d C_integral = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d C_doubleintegral = Eigen::Matrix3d::Zero();
  Eigen::Vector3d acc_integral = Eigen::Vector3d::Zero();
  Eigen::Vector3d acc_doubleintegral = Eigen::Vector3d::Zero();

  // cross matrix accumulatrion
  Eigen::Matrix3d cross = Eigen::Matrix3d::Zero();

  // sub-Jacobians
  Eigen::Matrix3d dalpha_db_g = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d dv_db_g = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d dp_db_g = Eigen::Matrix3d::Zero();

  // the Jacobian of the increment (w/o biases)
  Eigen::Matrix<double, 15, 15> P_delta = Eigen::Matrix<double, 15, 15>::Zero();

  double Delta_t = 0;
  bool hasStarted = false;
  int i = 0;

  for (okvis::ImuMeasurementDeque::const_iterator it = imuMeasurements.begin(); it != imuMeasurements.end(); ++it) {
    Eigen::Vector3d omega_S_0 = it->measurement.gyroscopes;
    Eigen::Vector3d acc_S_0 = it->measurement.accelerometers;
    Eigen::Vector3d omega_S_1 = (it + 1)->measurement.gyroscopes;
    Eigen::Vector3d acc_S_1 = (it + 1)->measurement.accelerometers;

    // time delta
    okvis::Time nexttime;
    if ((it + 1) == imuMeasurements.end()) {
      nexttime = t_end;
    } else {
      nexttime = (it + 1)->timeStamp;
    }
    double dt = (nexttime - time).toSec();