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

cleaned up notation

release/4.3a0
Timothy Setterfield 2020-09-02 07:59:19 -07:00
parent c3bddf1816
commit 688292f114
1 changed files with 19 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
gtsam/navigation/NavState.cpp
View File

@ -215,41 +215,40 @@ 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);
Vector9 xi;
Matrix3 D_dP_R;
dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0);
dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
dV(xi) << ((-2.0 * dt) * omega_cross_vel);
// Get perturbations in nav frame
Vector9 n_xi, xi;
Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav;
dR(n_xi) << ((-dt) * omega);
dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
dV(n_xi) << ((-2.0 * dt) * omega_cross_vel);
if (secondOrder) {
const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
dP(xi) -= (0.5 * dt2) * omega_cross2_t;
dV(xi) -= dt * omega_cross2_t;
dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
dV(n_xi) -= dt * omega_cross2_t;
}
// 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) = bRn * dP(xi);
dV(xi) = bRn * dV(xi);
// Transform n_xi into the body frame
xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0),
nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0),
nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0);
if (H) {
H->setZero();
const Matrix3 Omega = skewSymmetric(omega);
const Matrix3 D_cross_state = Omega * R();
H->setZero();
D_R_R(H) << D_dP_R;
D_t_v(H) << bRn * (-dt2) * D_cross_state;
D_t_R(H) << skewSymmetric(dP(xi));
D_v_v(H) << bRn * (-2.0 * dt) * D_cross_state;
D_v_R(H) << skewSymmetric(dV(xi));
D_R_R(H) << D_dR_R;
D_t_v(H) << D_body_nav * (-dt2) * D_cross_state;
D_t_R(H) << D_dP_R;
D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state;
D_v_R(H) << D_dV_R;
if (secondOrder) {
const Matrix3 D_cross2_state = Omega * D_cross_state;
D_t_t(H) -= bRn * (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= bRn * dt * D_cross2_state;
D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= D_body_nav * dt * D_cross2_state;
}
}
return xi;