diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 544c8b79b..aee6b6738 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -96,6 +96,7 @@ public: } /// Get so<3> version of bias corrected rotation, with optional Jacobian + // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) ) Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, OptionalJacobian<3, 3> H = boost::none) const { // First, we correct deltaRij using the biasOmegaIncr, rotated diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f007333e3..a38743c0e 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -266,8 +266,8 @@ public: // Get Get so<3> version of bias corrected rotation // If H5 is asked for, we will need the Jacobian, which we store in H5 // H5 will then be corrected below to take into account the Coriolis effect - Matrix3 H5temp; - Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &H5temp : 0); + Matrix3 D_cThetaRij_biasOmegaIncr; + Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); @@ -351,7 +351,7 @@ public: } if(H5) { // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * H5temp; + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; H5->resize(9,6); (*H5) << // dfP/dBias