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

Switched argument order

release/4.3a0
dellaert 2015-07-31 13:33:23 -07:00
parent 628a4cc4cc
commit e3d36da188
3 changed files with 20 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/navigation/NavState.cpp
View File

@ -238,7 +238,7 @@ Matrix7 NavState::wedge(const Vector9& xi) {
#define D_v_v(H) (H)->block<3,3>(6,6) #define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::update(const Vector3& omega, const Vector3& acceleration, NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const { OptionalJacobian<9, 3> G2) const {
@ -248,9 +248,9 @@ NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
double dt22 = 0.5 * dt * dt; double dt22 = 0.5 * dt * dt;
// Integrate on tangent space. TODO(frank): coriolis? // Integrate on tangent space. TODO(frank): coriolis?
dR(xi) << dt * omega; dR(xi) << dt * b_omega;
dP(xi) << dt * b_v + dt22 * acceleration; dP(xi) << dt * b_v + dt22 * b_acceleration;
dV(xi) << dt * acceleration; dV(xi) << dt * b_acceleration;
// Bring back to manifold // Bring back to manifold
Matrix9 D_newState_xi; Matrix9 D_newState_xi;
@ -261,21 +261,22 @@ NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
if (F) { if (F) {
F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state; F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state;
} }
// derivative wrt omega
if (G1) {
// D_newState_dRxi = D_newState_xi.leftCols<3>()
// D_dRxi_omega = dt * I_3x3
// *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega
*G1 = D_newState_xi.leftCols<3>() * dt;
}
// derivative wrt acceleration // derivative wrt acceleration
if (G2) { if (G1) {
// D_newState_dPxi = D_newState_xi.middleCols<3>(3) // D_newState_dPxi = D_newState_xi.middleCols<3>(3)
// D_dPxi_acc = dt22 * I_3x3 // D_dPxi_acc = dt22 * I_3x3
// D_newState_dVxi = D_newState_xi.rightCols<3>() // D_newState_dVxi = D_newState_xi.rightCols<3>()
// D_dVxi_acc = dt * I_3x3 // D_dVxi_acc = dt * I_3x3
// *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc
*G2 = D_newState_xi.middleCols<3>(3) * dt22 + D_newState_xi.rightCols<3>() * dt; *G1 = D_newState_xi.middleCols<3>(3) * dt22
+ D_newState_xi.rightCols<3>() * dt;
}
// derivative wrt omega
if (G2) {
// D_newState_dRxi = D_newState_xi.leftCols<3>()
// D_dRxi_omega = dt * I_3x3
// *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega
*G2 = D_newState_xi.leftCols<3>() * dt;
} }
return newState; return newState;
} }

4
gtsam/navigation/NavState.h
View File

@ -212,9 +212,9 @@ public:
/// @name Dynamics /// @name Dynamics
/// @{ /// @{
/// Integrate forward in time given angular velocity and acceleration /// Integrate forward in time given angular velocity and acceleration in body frame
/// Uses second order integration for position, returns derivatives except dt. /// Uses second order integration for position, returns derivatives except dt.
NavState update(const Vector3& omega, const Vector3& acceleration, NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const; OptionalJacobian<9, 3> G2) const;

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

@ -206,11 +206,11 @@ TEST(NavState, Update) {
NavState expected(kAttitude.expmap(deltaT * omega), NavState expected(kAttitude.expmap(deltaT * omega),
kPosition + Point3((kVelocity + b_acc * deltaT / 2) * deltaT), kPosition + Point3((kVelocity + b_acc * deltaT / 2) * deltaT),
kVelocity + b_acc * deltaT); kVelocity + b_acc * deltaT);
NavState actual = kState1.update(omega, acc, deltaT, aF, aG1, aG2); NavState actual = kState1.update(acc, omega, deltaT, aF, aG1, aG2);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(numericalDerivative31(update, kState1, omega, acc, 1e-7), aF, 1e-7)); EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7));
EXPECT(assert_equal(numericalDerivative32(update, kState1, omega, acc, 1e-7), aG1, 1e-7)); EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
EXPECT(assert_equal(numericalDerivative33(update, kState1, omega, acc, 1e-7), aG2, 1e-7)); EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
} }
/* ************************************************************************* */ /* ************************************************************************* */