From 1ce6ab893d7aea655a21ae3add45dfbb2d997973 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 20 Jul 2015 00:58:41 -0700 Subject: [PATCH] predict derivative works ! --- gtsam/navigation/PreintegrationBase.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 2c9b8b6ae..e60b14b05 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -252,11 +252,19 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i, NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { - Vector9 biasCorrected = biasCorrectedDelta(bias_i, H2); - Matrix9 D_delta_biasCorrected; - Vector9 delta = recombinedPrediction(state_i, biasCorrected, H1, H2 ? &D_delta_biasCorrected : 0); - if (H2) *H2 = D_delta_biasCorrected * (*H2); - return state_i.retract(delta); + Matrix96 D_biasCorrected_bias; + Vector9 biasCorrected = biasCorrectedDelta(bias_i, + H2 ? &D_biasCorrected_bias : 0); + Matrix9 D_delta_state, D_delta_biasCorrected; + Vector9 delta = recombinedPrediction(state_i, biasCorrected, + H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0); + Matrix9 D_predict_state, D_predict_delta; + NavState state_j = state_i.retract(delta, D_predict_state, D_predict_delta); + if (H1) + *H1 = D_predict_state + D_predict_delta * D_delta_state; + if (H2) + *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; + return state_j; } //------------------------------------------------------------------------------