Measurement Update dependent on cloned IMU state

[aserbremen]

First up, thanks for providing this amazing research platform.

I am trying to compare different update methodologies using vehicle speed and steering measurements for vehicles. One of the methods is supposed to be implement via odometry preintegration as in Section 2.1 of Visual-Inertial-Wheel odometry with Online Calibration. I am only interested in the odometry update without estimating odometry intrinsics and extrinsics.

I am having trouble to understand how to perform the EKF update with Jacobians dependent on the last IMU state (at the beginning of preintegration period) and the current IMU state (at the end of preintegration period). In particular I don't know how to apply the Jacobians equation (94) and equation (95) of Visual-Inertial-Wheel odometry with Online Calibration in the OpenVINS framework. Equation (94) is dependent on the IMU state and equation (95) on the state of IMU clones.

I am following the scheme of other update classes and came up with this:

  // Construct the Jacobians
  std::vector<std::shared_ptr<Type>> Hx_order;
  // The Jacobian entries correspond to the last and current IMU state, but since Hx_order is constructed by types only we take the types of
  // the current imu state
  Hx_order.push_back(state->_imu->q()); // eq (94) first row  
  Hx_order.push_back(state->_imu->p()); // eq (94) second row 
  Hx_order.push_back(state->_imu->q()); // eq (95) first row
  Hx_order.push_back(state->_imu->p()); // eq (95) second row

  // Large final matrices used for update
  int h_size = 12;
  int m_size = 3;
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(m_size, h_size);
  Eigen::Vector3d res;
  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(m_size, m_size);

  // Set the values of H ...

Is this the correct way of doing this? And is it possible with the current version of OpenVINS to perform an EKF update dependent on the cloned state? The above method of updating seems to work at times, but can also be quite unstable and state estimation diverges in these cases.

[goldbattle]

After propagation we perform state cloning. This means that we have a clone of the current IMU state in our _clones_IMU state vector. Thus you can simply grab the clone at the most recent time and the previous time:

 Hx_order.push_back(state->_clones_IMU->at(time0));
 Hx_order.push_back(state->_clones_IMU->at(time1));
 Hx_order.push_back(state->_wheel_calibration);

You would then integrate your wheel odometry between the time time time0 and time1. These would be the previous state->_timestamp and current state timestamp.

I am not sure how you are formulating a measurement to update with currently. With the code you have, both your derivatives are in respect to the current IMU state pose. Your Hx_order should reflect the Jacobian states you are taking derivatives in respect to exactly.

[aserbremen]

Thanks for your quick reply. I will test out your suggestion.

Right now I am saving the necessary variables I need to construct the measurement function as follows:

update_preintegration() {
  ...
  // construct measurement function using _last_imu_rot and _last_imu_pos 
  // as well as state->_imu->Rot() and state->_imu->pos()

  // save for next function call
  _last_imu_rot = state->_imu->Rot();
  _last_imu_pos = state->_imu->pos();
}

But if I understand you correctly, this is not necessary and I can rely on the cloned states

update_preintegration() {
  ...
  // construct measurement function using state->_clones_IMU->at(time0) as last IMU state 
  // as well as state->_clones_IMU->at(time1) as current IMU state

  // No saving of _last_imu_rot or _last_imu_pos needed
}

If this is correct, is there a difference between using state->_clones_IMU->at(time1) as the current IMU state and state->_imu->Rot(), state->_imu->pos(), because both should reflect the same thing at that point in time. Finally the Hx_order would be (I am not performing wheel calibration, hence leaving out state->_wheel_calibration):

 // using only cloned states
 Hx_order.push_back(state->_clones_IMU->at(time0));
 Hx_order.push_back(state->_clones_IMU->at(time1));

 // using cloned state and current IMU state
 Hx_order.push_back(state->_clones_IMU->at(time0));
 Hx_order.push_back(state->_imu->q());
 Hx_order.push_back(state->_imu->p());

[goldbattle]

You should stop yourself if you are saving any state values as these will change and update over time. If you are doing odometry / preintegration, then the state needs to have two clones. Conveniently there are camera clones you can use already. If these were not there then one would need to perform stochastic cloning to get them.

Both of your two examples would work as expected. In general we like to use the clones since this cleans up the logic to explicitly use the two timestamps you integrated between. You can directly get the JPL pose with _imu->pose() [see here].

[aserbremen]

I implemented the update using your suggestions and it seems to work fine now. Thanks again!