diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 42ea41b86..9c266e7a1 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -67,31 +67,28 @@ void PreintegratedCombinedMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::integrateMeasurement( 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, dt, - &D_incrR_integratedOmega, &A, &B, &C); + PreintegrationBase::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF - // framework. In this implementation, contrarily to [2] we consider the + // framework. In this implementation, in contrast to [2], we consider the // uncertainty of the bias selection and we keep correlation between biases // and preintegrated measurements // Single Jacobians to propagate covariance - Matrix3 H_vel_biasacc = -dRij * dt; - Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * dt; + // TODO(frank): should we not also accout for bias on position? + Matrix3 theta_H_biasOmega = - C.topRows<3>(); + Matrix3 vel_H_biasAcc = -B.bottomRows<3>(); // overall Jacobian wrt preintegrated measurements (df/dx) Eigen::Matrix F; F.setZero(); F.block<9, 9>(0, 0) = A; - F.block<3, 3>(0, 12) = H_angles_biasomega; - F.block<3, 3>(6, 9) = H_vel_biasacc; + F.block<3, 3>(0, 12) = theta_H_biasOmega; + F.block<3, 3>(6, 9) = vel_H_biasAcc; F.block<6, 6>(9, 9) = I_6x6; // propagate uncertainty @@ -101,24 +98,25 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( const Matrix3& iCov = p().integrationCovariance; // first order uncertainty propagation - // Optimized matrix multiplication (1/dt) * 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) = dt * iCov; - D_v_v(&G_measCov_Gt) = (1 / dt) * H_vel_biasacc * + D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) * - (H_vel_biasacc.transpose()); - D_R_R(&G_measCov_Gt) = (1 / dt) * H_angles_biasomega * + (vel_H_biasAcc.transpose()); + D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) * - (H_angles_biasomega.transpose()); + (theta_H_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().biasAccOmegaInt.block<3, 3>(3, 0) * - H_angles_biasomega.transpose(); + Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) * + theta_H_biasOmega.transpose(); D_v_R(&G_measCov_Gt) = temp; D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index aeda774d5..626b2f5c5 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -55,9 +55,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( // Update preintegrated measurements (also get Jacobian) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - Matrix3 D_incrR_integratedOmega; - PreintegrationBase::update(measuredAcc, measuredOmega, dt, - &D_incrR_integratedOmega, &A, &B, &C); + PreintegrationBase::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index f042aeae0..c96e39c9b 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -211,8 +211,7 @@ Vector9 PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc, //------------------------------------------------------------------------------ void PreintegrationBase::update(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, - Matrix3* D_incrR_integratedOmega, Matrix9* A, - Matrix93* B, Matrix93* C) { + Matrix9* A, Matrix93* B, Matrix93* C) { // Do update deltaTij_ += dt; preintegrated_ = updatedPreintegrated(measuredAcc, measuredOmega, dt, A, B, C); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 6d77cb3e0..2f3e9cd41 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -188,8 +188,7 @@ public: /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame void update(const Vector3& measuredAcc, const Vector3& measuredOmega, - const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* A, - Matrix93* B, Matrix93* C); + const double deltaT, Matrix9* A, Matrix93* B, Matrix93* C); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far