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

Second order coriolis done

release/4.3a0
dellaert 2015-07-19 23:57:00 -07:00
parent 5f6d3a600f
commit 2df20b4f37
2 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/navigation/PreintegrationBase.cpp
View File

@ -182,6 +182,7 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i,
const Pose3& pose_i = state_i.pose();
const Vector3& vel_i = state_i.velocity();
const Matrix3 Ri = pose_i.rotation().matrix();
const Vector3& t_i = state_i.translation().vector();
const double dt = deltaTij(), dt2 = dt * dt;
const Vector3& omegaCoriolis = *p().omegaCoriolis;
@ -190,16 +191,20 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i,
NavState::dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper
NavState::dV(result) -= 2.0 * omegaCoriolis.cross(vel_i) * dt;
if (p().use2ndOrderCoriolis) {
Vector3 temp = omegaCoriolis.cross(
omegaCoriolis.cross(pose_i.translation().vector()));
Vector3 temp = omegaCoriolis.cross(omegaCoriolis.cross(t_i));
NavState::dP(result) -= 0.5 * temp * dt2;
NavState::dV(result) -= temp * dt;
}
if (H) {
const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
H->block<3,3>(0,0) = -D_dP_Ri;
H->block<3,3>(3,6) = - omegaCoriolisHat * dt2;
H->block<3,3>(6,6) = - 2.0 * omegaCoriolisHat * dt;
H->block<3,3>(0,0) -= D_dP_Ri;
H->block<3,3>(3,6) -= omegaCoriolisHat * dt2;
H->block<3,3>(6,6) -= 2.0 * omegaCoriolisHat * dt;
if (p().use2ndOrderCoriolis) {
const Matrix3 omegaCoriolisHat2 = omegaCoriolisHat * omegaCoriolisHat;
H->block<3,3>(3,3) -= 0.5 * omegaCoriolisHat2 * dt2;
H->block<3,3>(6,3) -= omegaCoriolisHat2 * dt;
}
}
}
return result;

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

@ -218,7 +218,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
p->accelerometerCovariance = kMeasuredAccCovariance;
p->integrationCovariance = kIntegrationErrorCovariance;
p->use2ndOrderIntegration = false;
p->use2ndOrderCoriolis = false;
p->use2ndOrderCoriolis = true;
PreintegratedImuMeasurements pim(p, bias);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -236,7 +236,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
}
{
Vector9 expected; // TODO(frank): taken from output. Should really verify.
expected << -0.0212372436, -0.0407423986, -0.0974116854, 0.0, -0.08, 0.06, 0.0, -0.08, 0.06;
expected << -0.0212372436, -0.0407423986, -0.0974116854, 0.1038, 0.038, -0.0804, 0.1038, 0.038, -0.0804;
Matrix99 actualH;
EXPECT(assert_equal(expected, pim.integrateCoriolis(state1, actualH), 1e-5));
Matrix expectedH = numericalDerivative11<Vector9, NavState>(
@ -246,7 +246,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
}
{
Vector9 expected; // TODO(frank): taken from output. Should really verify.
expected << 0.0415946095, -0.0407423986, -0.0974116854, 1, -0.08, 9.85, -0.187214027, 0.110178303, 0.0436304821;
expected << 0.0415946095, -0.0407423986, -0.0974116854, 1.1038, 0.038, 9.7096, -0.0834140265, 0.228178303, -0.0967695179;
Matrix99 actualH1, actualH2;
Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias);
EXPECT(