From b0fcd17140cd3a9fb9abde60e4d52d18139a7145 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 17 Sep 2021 14:19:41 -0400 Subject: [PATCH] additional comments to make understanding the code easier --- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 216140dbc..1e99db17c 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -100,7 +100,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // Update preintegrated measurements. Matrix9 A; // Jacobian wrt preintegrated measurements without bias (df/dx) - Matrix93 B, C; + Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively. PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 28c0461b1..9b6affaaf 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( // Update preintegrated measurements (also get Jacobian) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix93 B, C; + Matrix93 B, C; // Jacobian of state wrpt accel bias and omega bias respectively. PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: @@ -73,11 +73,13 @@ void PreintegratedImuMeasurements::integrateMeasurement( const Matrix3& iCov = p().integrationCovariance; // (1/dt) allows to pass from continuous time noise to discrete time noise + // Update the uncertainty on the state (matrix A in [4]). preintMeasCov_ = A * preintMeasCov_ * A.transpose(); + // These 2 updates account for uncertainty on the IMU measurement (matrix B in [4]). preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose(); preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); - // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 + // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 (9x3 matrix) preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt; }