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

predict derivative works !

release/4.3a0
dellaert 2015-07-20 00:58:41 -07:00
parent 3a941a0ef4
commit 1ce6ab893d
1 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/navigation/PreintegrationBase.cpp
View File

@ -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;
}
//------------------------------------------------------------------------------