Use doublecross instead of cross(Cross(a, b)) in NavState#2287
Use doublecross instead of cross(Cross(a, b)) in NavState#2287dellaert merged 4 commits intoborglab:developfrom
Conversation
Change cross(cross(a, b)) to doubleCross(a, b)
gtsam/navigation/NavState.cpp
Outdated
| 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)); | ||
| const Vector3 omega_cross2_t = doubleCross(omega, n_t); |
There was a problem hiding this comment.
I think switching to doubleCross is most beneficial if we also grab its Jacobians and use them below to replace some complexity in the Jacobian chaining...
There was a problem hiding this comment.
I agree. I did a bit of tests and had one term not matching (probably because I didn't check myself and just trusted copilot) so I will look again tonight
There's also some doublecrosses elsewhere I believe
There was a problem hiding this comment.
Some of this code was rearranged. I wanted to only create doubleCross() once which might be the wrong approach. So H2_wrt_t is created even if we don't use it. Not sure.
Change cross(cross(a, b)) to doubleCross(a, b)
- Remove excess white space
scottiyio
changed the title
gtsam/navigation/NavState.cpp
Outdated
| // Get perturbations in nav frame | ||
| Vector9 n_xi, xi; | ||
| Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav; | ||
| // Because our navigation frames are placed on a spinning Earth, we experience two apparent forces on our inertials |
gtsam/navigation/NavState.cpp
Outdated
| 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); | ||
|
|
||
| // Centrifugal (second order) acceleration corrections | ||
| Matrix3 H2_wrt_t; // To store Jacobian (if needed/desired) |
gtsam/navigation/NavState.cpp
Outdated
| nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0); | ||
| // Transform correction from navigation frame -> body frame and get Jacobians | ||
| Vector9 xi; | ||
| Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav; |
There was a problem hiding this comment.
D_body_nav -> Rt, and remark that second derivative is the same for all three.
gtsam/navigation/NavState.cpp
Outdated
| const Matrix3 D_cross2_state = Omega * D_cross_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; | ||
| D_t_t(H) -= (0.5 * dt2) * D_body_nav * (H2_wrt_t * R()); |
There was a problem hiding this comment.
D_p_p(H) -= (0.5 * dt2) * D_c2_np; ???
gtsam/navigation/NavState.cpp
Outdated
| D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state; | ||
| D_v_t(H) -= D_body_nav * dt * D_cross2_state; | ||
| D_t_t(H) -= (0.5 * dt2) * D_body_nav * (H2_wrt_t * R()); | ||
| D_v_t(H) -= dt * D_body_nav * (H2_wrt_t * R()); |
There was a problem hiding this comment.
dt * D_c2_np; ????
I'd add a unit test with non-identity R before - make sure it fails - then apply fix, should succeed.
|
Would love to merge this! |
…larations. Rotated omega (earth angular rate, not IMU angular velocities) to the body frame for simplification Cannot rename "D_t_t" to say "D_p_p" or "D_v_t" to "D_v_p" because of NavState derivatives?
2 participants
Change cross(cross(a, b)) to doubleCross(a, b) and add documentation
Add a no rotation coriolis test to NavState