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

test centrifugal derivative

release/4.3a0
Duy-Nguyen Ta 2015-09-02 16:52:54 -04:00
parent a4e58d06e7
commit 9f91aedd6a
1 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -157,6 +157,35 @@ bool MonteCarlo(const PreintegratedImuMeasurements& pim,
return assert_equal(Matrix(10000*Q), 10000*actual, 1);
}
/* ************************************************************************* */
Vector3 centrifugal(const Vector3& omega_s, const Pose3& bTs, boost::optional<Matrix33&> H1) {
Rot3 bRs = bTs.rotation();
Vector3 j_correctedOmega = bRs * omega_s;
const Vector3 b_arm = bTs.translation().vector();
// Subtract out the the centripetal acceleration from the measured one
// to get linear acceleration vector in the body frame:
const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
if (H1) {
double wdp = j_correctedOmega.dot(b_arm);
*H1 = - (diag(Vector3::Constant(wdp)) + j_correctedOmega * b_arm.transpose())*bRs.matrix() + 2*b_arm*omega_s.transpose();
}
return -body_Omega_body * b_velocity_bs;
}
/* ************************************************************************* */
TEST(ImuFactor, centrifugalDeriv) {
static const Pose3 bTs(Rot3::ypr(0.1,0.2,0.3), Point3(1.,4.,7.));
Vector3 omega_s(0.2,0.4,0.6);
Matrix3 H1;
Vector3 cb = centrifugal(omega_s, bTs, H1);
(void) cb;
Matrix Hnum = numericalDerivative11<Vector3, Vector3>(boost::bind(centrifugal, _1, bTs, boost::none), omega_s, 1e-6);
EXPECT(assert_equal(Hnum, H1, 1e-5));
}
/* ************************************************************************* */
TEST(ImuFactor, StraightLine) {
// Set up IMU measurements