From 87446914dfd6d32b59ccc09aebc17ff56cb3726f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 4 Jun 2016 16:22:04 -0700 Subject: [PATCH] Clarifying comments --- gtsam/navigation/TangentPreintegration.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 2df958d05..0ac57d05c 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -128,10 +128,14 @@ void TangentPreintegration::update(const Vector3& measuredAcc, *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } - // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias + // new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc + // where acc_H_biasAcc = -I_3x3, hence + // new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); - // D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias + // new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega + // where omega_H_biasOmega = -I_3x3, hence + // new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); }