diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 21bfcbd1e..b65810aae 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -66,14 +66,14 @@ void PreintegratedCombinedMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) { + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { const Matrix3 dRij = deltaRij().matrix(); // expensive when quaternion // Update preintegrated measurements. Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::update(measuredAcc, measuredOmega, deltaT, + PreintegrationBase::update(measuredAcc, measuredOmega, dt, &D_incrR_integratedOmega, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first @@ -83,8 +83,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // and preintegrated measurements // Single Jacobians to propagate covariance - Matrix3 H_vel_biasacc = -dRij * deltaT; - Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT; + Matrix3 H_vel_biasacc = -dRij * dt; + Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * dt; // overall Jacobian wrt preintegrated measurements (df/dx) Eigen::Matrix F; @@ -94,24 +94,27 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( F.block<3, 3>(6, 9) = H_vel_biasacc; F.block<6, 6>(9, 9) = I_6x6; + // propagate uncertainty + // TODO(frank): use noiseModel routine so we can have arbitrary noise models. + const Matrix3& aCov = p().accelerometerCovariance; + const Matrix3& wCov = p().gyroscopeCovariance; + const Matrix3& iCov = p().integrationCovariance; + // first order uncertainty propagation - // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * - // G.transpose() + // Optimized matrix multiplication (1/dt) * G * measurementCovariance * G.transpose() Eigen::Matrix G_measCov_Gt; G_measCov_Gt.setZero(15, 15); // BLOCK DIAGONAL TERMS - D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance; - D_v_v(&G_measCov_Gt) = - (1 / deltaT) * (H_vel_biasacc) * - (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0)) * - (H_vel_biasacc.transpose()); - D_R_R(&G_measCov_Gt) = - (1 / deltaT) * (H_angles_biasomega) * - (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3)) * - (H_angles_biasomega.transpose()); - D_a_a(&G_measCov_Gt) = deltaT * p().biasAccCovariance; - D_g_g(&G_measCov_Gt) = deltaT * p().biasOmegaCovariance; + D_t_t(&G_measCov_Gt) = dt * iCov; + D_v_v(&G_measCov_Gt) = (1 / dt) * H_vel_biasacc * + (aCov + p().biasAccOmegaInit.block<3, 3>(0, 0)) * + (H_vel_biasacc.transpose()); + D_R_R(&G_measCov_Gt) = (1 / dt) * H_angles_biasomega * + (wCov + p().biasAccOmegaInit.block<3, 3>(3, 3)) * + (H_angles_biasomega.transpose()); + D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; + D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) *