diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 142faedd3..e5ce5a711 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -74,9 +74,6 @@ bool PreintegratedCombinedMeasurements::equals( void PreintegratedCombinedMeasurements::resetIntegration() { // Base class method to reset the preintegrated measurements PreintegrationType::resetIntegration(); - // Set the initial bias covariance to - // the estimated covariance from the last step. - p().biasAccOmegaInt = preintMeasCov_.block<6, 6>(9, 9); preintMeasCov_.setZero(); }