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

added cached rotation bRn

release/4.3a0
Timothy Setterfield 2020-09-01 13:07:51 -07:00
parent cb6573ad1f
commit c3bddf1816
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/navigation/NavState.cpp
View File

@ -215,6 +215,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
OptionalJacobian<9, 9> H) const {
TIE(nRb, n_t, n_v, *this);
Matrix3 bRn = nRb.matrix().transpose();
const double dt2 = dt * dt;
const Vector3 omega_cross_vel = omega.cross(n_v);
@ -231,8 +232,8 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
// NOTE(tim): position and velocity changes dP and dV are in nav frame; need
// to put them into local frame for subsequent retract
dP(xi) = nRb.unrotate(dP(xi));
dV(xi) = nRb.unrotate(dV(xi));
dP(xi) = bRn * dP(xi);
dV(xi) = bRn * dV(xi);
if (H) {
@ -241,14 +242,14 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
const Matrix3 D_cross_state = Omega * R();
H->setZero();
D_R_R(H) << D_dP_R;
D_t_v(H) << nRb.matrix().transpose() * (-dt2) * D_cross_state;
D_t_v(H) << bRn * (-dt2) * D_cross_state;
D_t_R(H) << skewSymmetric(dP(xi));
D_v_v(H) << nRb.matrix().transpose() * (-2.0 * dt) * D_cross_state;
D_v_v(H) << bRn * (-2.0 * dt) * D_cross_state;
D_v_R(H) << skewSymmetric(dV(xi));
if (secondOrder) {
const Matrix3 D_cross2_state = Omega * D_cross_state;
D_t_t(H) -= nRb.matrix().transpose() * (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= nRb.matrix().transpose() * dt * D_cross2_state;
D_t_t(H) -= bRn * (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= bRn * dt * D_cross2_state;
}
}
return xi;