From cefc441fba1ba8d6e0ea75d03e85b89f6bdc491e Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 30 Jul 2015 09:39:25 -0700 Subject: [PATCH] bodyVelocity and working update derivatives --- gtsam/navigation/NavState.cpp | 71 +++++++++++++------------ gtsam/navigation/NavState.h | 15 +++--- gtsam/navigation/tests/testNavState.cpp | 32 +++++++---- 3 files changed, 70 insertions(+), 48 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 9f21001b8..59f381659 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -55,6 +55,17 @@ const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const { return v_; } +//------------------------------------------------------------------------------ +Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { + const Rot3& nRb = R_; + const Vector3& n_v = v_; + Matrix3 D_bv_nRb; + Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0); + if (H) + *H << D_bv_nRb, Z_3x3, I_3x3; + return b_v; +} + //------------------------------------------------------------------------------ Matrix7 NavState::matrix() const { Matrix3 R = this->R(); @@ -228,44 +239,38 @@ Matrix7 NavState::wedge(const Vector9& xi) { //------------------------------------------------------------------------------ NavState NavState::update(const Vector3& omega, const Vector3& acceleration, - const double deltaT, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { - TIE(nRb, n_t, n_v, *this); + Vector9 xi; + Matrix39 D_xiP_state; + double dt22 = 0.5 * dt * dt; + dR(xi) << dt * omega; + dP(xi) << dt * bodyVelocity(D_xiP_state) + dt22 * acceleration; + dV(xi) << dt * acceleration; - // Calculate acceleration in *current* i frame, i.e., before rotation update below - Matrix3 D_acc_R; - const Matrix3 dRij = R(); // expensive in quaternion case - const Vector3 i_acc = nRb.rotate(acceleration, D_acc_R); + Matrix9 D_result_xi; + NavState result = retract(xi, F, D_result_xi); - // rotation vector describing rotation increment computed from the - // current rotation rate measurement - const Vector3 integratedOmega = omega * deltaT; - Matrix3 D_incrR_integratedOmega; - Rot3 incrR = Rot3::Expmap(integratedOmega, G1 ? &D_incrR_integratedOmega : 0); // expensive !! - - Matrix3 D_Rij_incrR; - double dt22 = 0.5 * deltaT * deltaT; - /// TODO(frank): use retract(deltaT*[omega, bRn*n_v * 0.5*deltaT*acceleration, acceleration]) - /// But before we do, figure out retract derivatives that are nicer than Lie-generated ones - NavState result(nRb.compose(incrR, D_Rij_incrR), - n_t + Point3(dt22 * i_acc + deltaT * n_v), n_v + deltaT * i_acc); - - // derivative wrt state + // Derivative wrt state is computed by retract directly + // However, as xi also depends on n_v, we need to add that contribution if (F) { - F->setIdentity(); - D_R_R(F) << D_Rij_incrR; - D_t_R(F) << dt22 * D_acc_R; - D_t_v(F) << I_3x3 * deltaT; - D_v_R(F) << deltaT * D_acc_R; + Matrix9 D_xi_state; + D_xi_state.setZero(); + D_xi_state.middleRows<3>(3) = dt * D_xiP_state; + *F += D_result_xi * D_xi_state; } // derivative wrt omega if (G1) { - *G1 << D_incrR_integratedOmega * deltaT, Z_3x3, Z_3x3; + Matrix93 D_xi_omega; + D_xi_omega << dt * I_3x3, Z_3x3, Z_3x3; + *G1 = D_result_xi * D_xi_omega; } // derivative wrt acceleration if (G2) { - *G2 << Z_3x3, dt22 * dRij, dRij * deltaT; + Matrix93 D_xi_acceleration; + D_xi_acceleration << Z_3x3, dt22 * I_3x3, dt * I_3x3; + *G2 = D_result_xi * D_xi_acceleration; } return result; } @@ -313,16 +318,16 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, const Velocity3& n_v = v_; // derivative is Ri ! const double dt2 = dt * dt; - Vector9 delta; + Vector9 xi; Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri; - dR(delta) = dR(pim); - dP(delta) = dP(pim) + dR(xi) = dR(pim); + dP(xi) = dP(pim) + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0) + (0.5 * dt2) * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0); - dV(delta) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0); + dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0); if (omegaCoriolis) { - delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); + xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); } if (H1 || H2) { @@ -340,7 +345,7 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, } } - return delta; + return xi; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index faf6a9c9f..f1e850037 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -100,6 +100,10 @@ public: Matrix3 R() const { return R_.matrix(); } + /// Return quaternion. Induces computation in matrix mode + Quaternion quaternion() const { + return R_.toQuaternion(); + } /// Return position as Vector3 Vector3 t() const { return t_.vector(); @@ -108,10 +112,9 @@ public: const Vector3& v() const { return v_; } - /// Return quaternion. Induces computation in matrix mode - Quaternion quaternion() const { - return R_.toQuaternion(); - } + // Return velocity in body frame + Velocity3 bodyVelocity(OptionalJacobian<3, 9> H) const; + /// Return matrix group representation, in MATLAB notation: /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] /// With this embedding in GL(3), matrix product agrees with compose @@ -210,9 +213,9 @@ public: /// @{ /// Integrate forward in time given angular velocity and acceleration - /// Uses second order integration for position, returns derivatives except deltaT. + /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& omega, const Vector3& acceleration, - const double deltaT, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const; /// Compute tangent space contribution due to Coriolis forces diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 986b9aa13..a885936aa 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -24,12 +24,12 @@ using namespace std; using namespace gtsam; -static const Rot3 kRotation = Rot3::RzRyRx(0.1, 0.2, 0.3); +static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3); static const Point3 kPosition(1.0, 2.0, 3.0); -static const Pose3 kPose(kRotation, kPosition); +static const Pose3 kPose(kAttitude, kPosition); static const Velocity3 kVelocity(0.4, 0.5, 0.6); static const NavState kIdentity; -static const NavState kState1(kRotation, kPosition, kVelocity); +static const NavState kState1(kAttitude, kPosition, kVelocity); static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); static const Vector3 kGravity(0, 0, 9.81); static const Vector9 kZeroXi = Vector9::Zero(); @@ -46,15 +46,17 @@ TEST(NavState, Constructor) { EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1)); EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); } + /* ************************************************************************* */ TEST( NavState, Attitude) { Matrix39 aH, eH; Rot3 actual = kState1.attitude(aH); - EXPECT(assert_equal(actual, kRotation)); + EXPECT(assert_equal(actual, kAttitude)); eH = numericalDerivative11( boost::bind(&NavState::attitude, _1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } + /* ************************************************************************* */ TEST( NavState, Position) { Matrix39 aH, eH; @@ -64,6 +66,7 @@ TEST( NavState, Position) { boost::bind(&NavState::position, _1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } + /* ************************************************************************* */ TEST( NavState, Velocity) { Matrix39 aH, eH; @@ -73,6 +76,17 @@ TEST( NavState, Velocity) { boost::bind(&NavState::velocity, _1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } + +/* ************************************************************************* */ +TEST( NavState, BodyVelocity) { + Matrix39 aH, eH; + Velocity3 actual = kState1.bodyVelocity(aH); + EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); + eH = numericalDerivative11( + boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); + EXPECT(assert_equal((Matrix )eH, aH)); +} + /* ************************************************************************* */ TEST( NavState, MatrixGroup ) { // check roundtrip conversion to 7*7 matrix representation @@ -188,15 +202,15 @@ TEST(NavState, Update) { boost::function update = boost::bind(&NavState::update, _1, _2, _3, deltaT, boost::none, boost::none, boost::none); - Vector3 b_acc = kRotation * acc; - NavState expected(kRotation.expmap(deltaT * omega), + Vector3 b_acc = kAttitude * acc; + NavState expected(kAttitude.expmap(deltaT * omega), kPosition + Point3((kVelocity + b_acc * deltaT / 2) * deltaT), kVelocity + b_acc * deltaT); NavState actual = kState1.update(omega, acc, deltaT, aF, aG1, aG2); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(numericalDerivative31(update, kState1, omega, acc), aF)); - EXPECT(assert_equal(numericalDerivative32(update, kState1, omega, acc), aG1)); - EXPECT(assert_equal(numericalDerivative33(update, kState1, omega, acc), aG2)); + EXPECT(assert_equal(numericalDerivative31(update, kState1, omega, acc, 1e-7), aF, 1e-7)); + EXPECT(assert_equal(numericalDerivative32(update, kState1, omega, acc, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal(numericalDerivative33(update, kState1, omega, acc, 1e-7), aG2, 1e-7)); } /* ************************************************************************* */