IMU Integration bug with non-zero body_P_sensor?

I'm getting some strange results when using a non-zero body_P_sensor.

I think there's an error in PreintegrationBase::correctMeasurementsBySensorPose(). In the calculation of centrifugal accelerations there is:

    // Subtract out the the centripetal acceleration from the unbiased one
    // to get linear acceleration vector in the body frame:
    const Matrix3 body_Omega_body = skewSymmetric(correctedOmega);
    const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
    correctedAcc -= body_Omega_body * b_velocity_bs;

I think it should be:

    // Subtract out the the centripetal acceleration from the unbiased one
    // to get linear acceleration vector in the body frame:
    const Matrix3 body_Omega_body = skewSymmetric(correctedOmega);
    const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
    correctedAcc -= b_velocity_bs;

Then in the calculation of the jacobians, I think it should just be:

*correctedAcc_H_unbiasedOmega = skewSymmetric(b_arm);

If I make these changes, it seems to work. Do you agree?

Comments (3)