diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 2dfaae2fd..64d555e2e 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -145,9 +145,9 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // BLOCK DIAGONAL TERMS 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()); + (theta_H_omega * (wCov / dt) * theta_H_omega.transpose()) // + + (theta_H_biasOmegaInit * (bInitCov.block<3, 3>(3, 3) / dt) * + theta_H_biasOmegaInit.transpose()); D_t_t(&G_measCov_Gt) = (pos_H_acc * (aCov / dt) * pos_H_acc.transpose()) // diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 3a447dcab..9d3e258de 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -145,7 +145,8 @@ Eigen::Matrix CombinedScenarioRunner::estimateCovariance( auto pim = integrate(T, estimatedBias, true); NavState sampled = predict(pim); Vector15 xi = Vector15::Zero(); - xi << sampled.localCoordinates(prediction), estimatedBias_.vector(); + xi << sampled.localCoordinates(prediction), + (estimatedBias_.vector() - estimatedBias.vector()); samples.col(i) = xi; sum += xi; }