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

Better documentation, and conditional evaluation of derivatives

release/4.3a0
dellaert 2015-07-30 21:55:40 -07:00
parent 2ad50ab86e
commit 91eeede05a
1 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/navigation/NavState.cpp
View File

@ -246,11 +246,11 @@ NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
Matrix39 D_xiP_state; Matrix39 D_xiP_state;
double dt22 = 0.5 * dt * dt; double dt22 = 0.5 * dt * dt;
dR(xi) << dt * omega; dR(xi) << dt * omega;
dP(xi) << dt * bodyVelocity(D_xiP_state) + dt22 * acceleration; dP(xi) << dt * bodyVelocity(F ? &D_xiP_state : 0) + dt22 * acceleration;
dV(xi) << dt * acceleration; dV(xi) << dt * acceleration;
Matrix9 D_result_xi; Matrix9 D_result_xi;
NavState result = retract(xi, F, D_result_xi); NavState result = retract(xi, F, G1 || G2 ? &D_result_xi : 0);
// Derivative wrt state is computed by retract directly // Derivative wrt state is computed by retract directly
// However, as dP(xi) also depends on state, we need to add that contribution // However, as dP(xi) also depends on state, we need to add that contribution
@ -259,11 +259,19 @@ NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
} }
// derivative wrt omega // derivative wrt omega
if (G1) { if (G1) {
*G1 = dt * D_result_xi.leftCols<3>(); // D_result_dRxi = D_result_xi.leftCols<3>()
// D_dRxi_omega = dt * I_3x3
// *G1 = D_result_omega = D_result_dRxi * D_dRxi_omega
*G1 = D_result_xi.leftCols<3>() * dt;
} }
// derivative wrt acceleration // derivative wrt acceleration
if (G2) { if (G2) {
*G2 = dt22 * D_result_xi.middleCols<3>(3) + dt * D_result_xi.rightCols<3>(); // D_result_dPxi = D_result_xi.middleCols<3>(3)
// D_dPxi_acc = dt22 * I_3x3
// D_result_dVxi = D_result_xi.rightCols<3>()
// D_dVxi_acc = dt * I_3x3
// *G2 = D_result_acc = D_result_dPxi * D_dPxi_acc + D_result_dVxi * D_dVxi_acc
*G2 = D_result_xi.middleCols<3>(3) * dt22 + D_result_xi.rightCols<3>() * dt;
} }
return result; return result;
} }