12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

test passes

release/4.3a0
Varun Agrawal 2021-09-30 09:01:23 -04:00
parent e38ea502d7
commit 3cee1b71ec
2 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -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()) //

3
gtsam/navigation/ScenarioRunner.cpp
View File

@ -145,7 +145,8 @@ Eigen::Matrix<double, 15, 15> 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;
}