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

Merge pull request #505 from tsetterf/fix/coriolis-integration 

Fixed issue with Coriolis acceleration
release/4.3a0
Frank Dellaert 2020-09-02 14:24:04 -04:00 committed by GitHub
parent 02cc96de01 335b57ca36
commit 915702116f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 64 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/navigation/NavState.cpp
View File

@ -218,28 +218,37 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
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;
}
// 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) << (-dt2) * D_cross_state;
D_v_v(H) << (-2.0 * dt) * D_cross_state;
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) -= (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= 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;

43
gtsam/navigation/tests/testNavState.cpp
View File

@ -192,6 +192,49 @@ TEST(NavState, Coriolis2) {
EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
}
TEST(NavState, Coriolis3) {
/** Consider a massless planet with an attached nav frame at
* n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with
* velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously
* aligned with the nav frame (i.e., nRb != I_3x3). Test that first and
* second order Coriolis corrections are as expected.
*/
// Get true first and second order coriolis accelerations
double dt = 2.0, dt2 = dt * dt;
Vector3 n_omega(0.0, 0.0, 1.0), n_t(1.0, 0.0, 0.0), n_v(0.0, 1.0, 0.0);
Vector3 n_aCorr1 = -2.0 * n_omega.cross(n_v),
n_aCorr2 = -n_omega.cross(n_omega.cross(n_t));
Rot3 nRb = Rot3(-1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0),
bRn = nRb.inverse();
// Get expected first and second order corrections in the nav frame
Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1,
n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2),
n_dV1e = dt * n_aCorr1,
n_dV2e = dt * (n_aCorr1 + n_aCorr2);
// Get expected first and second order corrections in the body frame
Vector3 dRe = -dt * (bRn * n_omega),
b_dP1e = bRn * n_dP1e, b_dP2e = bRn * n_dP2e,
b_dV1e = bRn * n_dV1e, b_dV2e = bRn * n_dV2e;
// Get actual first and scond order corrections in body frame
NavState kState2(nRb, n_t, n_v);
Vector9 dXi1a = kState2.coriolis(dt, n_omega, false),
dXi2a = kState2.coriolis(dt, n_omega, true);
Vector3 dRa = NavState::dR(dXi1a),
b_dP1a = NavState::dP(dXi1a), b_dV1a = NavState::dV(dXi1a),
b_dP2a = NavState::dP(dXi2a), b_dV2a = NavState::dV(dXi2a);
EXPECT(assert_equal(dRe, dRa));
EXPECT(assert_equal(b_dP1e, b_dP1a));
EXPECT(assert_equal(b_dV1e, b_dV1a));
EXPECT(assert_equal(b_dP2e, b_dP2a));
EXPECT(assert_equal(b_dV2e, b_dV2a));
}
/* ************************************************************************* */
TEST(NavState, CorrectPIM) {
Vector9 xi;