8000 Fixed issue with Coriolis acceleration by tsetterf · Pull Request #505 · borglab/gtsam · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

Fixed issue with Coriolis acceleration #505

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 5 commits into from
Sep 2, 2020
Merged

Fixed issue with Coriolis acceleration #505

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
0c62a95
fixed issue that pos and vel are in nav frame
tsetterf Sep 1, 2020
cb6573a
added another coriolis unit test
tsetterf Sep 1, 2020
c3bddf1
added cached rotation bRn
tsetterf Sep 1, 2020
688292f
cleaned up notation
tsetterf Sep 2, 2020
335b57c
fixed typo
tsetterf Sep 2, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 21 additions & 12 deletions gtsam/navigation/NavState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
43 changes: 43 additions & 0 deletions gtsam/navigation/tests/testNavState.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
0