diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 5d49e4c0b..2dfaae2fd 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -110,17 +110,22 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // and preintegrated measurements // Single Jacobians to propagate covariance - Matrix3 theta_H_biasOmega = C.topRows<3>(); - Matrix3 pos_H_biasAcc = B.middleRows<3>(3); - Matrix3 vel_H_biasAcc = B.bottomRows<3>(); + Matrix3 theta_H_omega = C.topRows<3>(); + Matrix3 pos_H_acc = B.middleRows<3>(3); + Matrix3 vel_H_acc = B.bottomRows<3>(); + + Matrix3 theta_H_biasOmegaInit = -theta_H_omega; + Matrix3 pos_H_biasAccInit = -pos_H_acc; + Matrix3 vel_H_biasAcc = -vel_H_acc; + Matrix3 vel_H_biasAccInit = -vel_H_acc; // 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) = theta_H_biasOmega; - F.block<3, 3>(3, 9) = pos_H_biasAcc; - F.block<3, 3>(6, 9) = vel_H_biasAcc; + F.block<3, 3>(0, 12) = theta_H_omega; + F.block<3, 3>(3, 9) = pos_H_acc; + F.block<3, 3>(6, 9) = vel_H_acc; F.block<6, 6>(9, 9) = I_6x6; // Update the uncertainty on the state (matrix F in [4]). @@ -131,37 +136,52 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( const Matrix3& aCov = p().accelerometerCovariance; const Matrix3& wCov = p().gyroscopeCovariance; const Matrix3& iCov = p().integrationCovariance; + const Matrix6& bInitCov = p().biasAccOmegaInit; // first order uncertainty propagation // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose() Eigen::Matrix G_measCov_Gt; G_measCov_Gt.setZero(15, 15); - Matrix3 aCov_updated = aCov + p().biasAccOmegaInit.block<3, 3>(0, 0); - Matrix3 wCov_updated = wCov + p().biasAccOmegaInit.block<3, 3>(3, 3); - // BLOCK DIAGONAL TERMS - D_t_t(&G_measCov_Gt) = (pos_H_biasAcc // - * (aCov_updated / dt) // - * pos_H_biasAcc.transpose()) + (dt * iCov); - D_v_v(&G_measCov_Gt) = vel_H_biasAcc // - * (aCov_updated / dt) // - * (vel_H_biasAcc.transpose()); + D_R_R(&G_measCov_Gt) = + (theta_H_omega * (wCov / dt) * theta_H_omega.transpose()) + // + (theta_H_biasOmegaInit * (bInitCov.block<3, 3>(3, 3) / dt) * + theta_H_biasOmegaInit.transpose()); - D_R_R(&G_measCov_Gt) = theta_H_biasOmega // - * (wCov_updated / dt) // - * (theta_H_biasOmega.transpose()); + D_t_t(&G_measCov_Gt) = + (pos_H_acc * (aCov / dt) * pos_H_acc.transpose()) // + + (pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) * + pos_H_biasAccInit.transpose()) // + + (dt * iCov); + + D_v_v(&G_measCov_Gt) = + (vel_H_acc * (aCov / dt) * vel_H_acc.transpose()) // + + (vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) * + vel_H_biasAccInit.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 = vel_H_biasAcc * p().biasAccOmegaInit.block<3, 3>(3, 0) - * theta_H_biasOmega.transpose(); - D_v_R(&G_measCov_Gt) = temp; - D_v_t(&G_measCov_Gt) = vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose(); - D_R_v(&G_measCov_Gt) = temp.transpose(); - D_t_v(&G_measCov_Gt) = pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose(); + D_R_t(&G_measCov_Gt) = theta_H_biasOmegaInit * + (bInitCov.block<3, 3>(3, 0) / dt) * + pos_H_biasAccInit.transpose(); + D_R_v(&G_measCov_Gt) = theta_H_biasOmegaInit * + (bInitCov.block<3, 3>(3, 0) / dt) * + vel_H_biasAccInit.transpose(); + D_t_R(&G_measCov_Gt) = pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 3) / dt) * + theta_H_biasOmegaInit.transpose(); + D_t_v(&G_measCov_Gt) = + (pos_H_acc * (aCov / dt) * vel_H_acc.transpose()) + + (pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) * + vel_H_biasAccInit.transpose()); + D_v_R(&G_measCov_Gt) = vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 3) / dt) * + theta_H_biasOmegaInit.transpose(); + D_v_t(&G_measCov_Gt) = + (vel_H_acc * (aCov / dt) * pos_H_acc.transpose()) + + (vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) * + pos_H_biasAccInit.transpose()); preintMeasCov_.noalias() += G_measCov_Gt; }