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

bodyVelocity and working update derivatives

release/4.3a0
dellaert 2015-07-30 09:39:25 -07:00
parent 13f8935c52
commit cefc441fba
3 changed files with 70 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
gtsam/navigation/NavState.cpp
View File

@ -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;
}
//------------------------------------------------------------------------------

15
gtsam/navigation/NavState.h
View File

@ -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

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

@ -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<Rot3, NavState>(
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<Velocity3, NavState>(
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<NavState(const NavState&, const Vector3&, const Vector3&)> 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));
}
/* ************************************************************************* */