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

First order coriolis derivatives

release/4.3a0
dellaert 2015-07-19 23:40:20 -07:00
parent a56c6e728b
commit 5f6d3a600f
3 changed files with 112 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
gtsam/navigation/PreintegrationBase.cpp
View File

@ -155,8 +155,29 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const {
static Vector3 rotate(const Matrix3& R, const Vector3& p,
OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) {
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R;
return R * p;
}
//------------------------------------------------------------------------------
static Vector3 unrotate(const Matrix3& R, const Vector3& p,
OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) {
const Matrix3 Rt = R.transpose();
Vector3 q = Rt * p;
const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1) *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
if (H2) *H2 = Rt;
return q;
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i,
OptionalJacobian<9, 9> H) const {
Vector9 result = Vector9::Zero();
if (H) H->setZero();
if (p().omegaCoriolis) {
const Pose3& pose_i = state_i.pose();
const Vector3& vel_i = state_i.velocity();
@ -164,22 +185,29 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const {
const double dt = deltaTij(), dt2 = dt * dt;
const Vector3& omegaCoriolis = *p().omegaCoriolis;
NavState::dR(result) -= Ri.transpose() * omegaCoriolis * dt;
Matrix3 D_dP_Ri;
NavState::dR(result) -= unrotate(Ri, omegaCoriolis * dt, H ? &D_dP_Ri : 0);
NavState::dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper
NavState::dV(result) -= 2 * omegaCoriolis.cross(vel_i) * dt;
NavState::dV(result) -= 2.0 * omegaCoriolis.cross(vel_i) * dt;
if (p().use2ndOrderCoriolis) {
Vector3 temp = omegaCoriolis.cross(
omegaCoriolis.cross(pose_i.translation().vector()));
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;
}
}
return result;
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1,
const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) const {
const Pose3& pose_i = state_i.pose();
@ -189,11 +217,29 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
// Rotation, translation, and velocity:
Vector9 delta;
Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc;
NavState::dR(delta) = NavState::dR(biasCorrectedDelta);
NavState::dP(delta) = Ri * NavState::dP(biasCorrectedDelta) + vel_i * dt + 0.5 * p().gravity * dt2;
NavState::dV(delta) = Ri * NavState::dV(biasCorrectedDelta) + p().gravity * dt;
NavState::dP(delta) = rotate(Ri, NavState::dP(biasCorrectedDelta), D_dP_Ri, D_dP_bc) + vel_i * dt + 0.5 * p().gravity * dt2;
NavState::dV(delta) = rotate(Ri, NavState::dV(biasCorrectedDelta), D_dV_Ri, D_dV_bc) + p().gravity * dt;
Matrix9 Hcoriolis;
if (p().omegaCoriolis) {
delta += integrateCoriolis(state_i, H1 ? &Hcoriolis : 0);
}
if (H1) {
H1->setZero();
H1->block<3,3>(3,0) = D_dP_Ri;
H1->block<3,3>(3,6) = I_3x3 * dt;
H1->block<3,3>(6,0) = D_dV_Ri;
if (p().omegaCoriolis) *H1 += Hcoriolis;
}
if (H2) {
H2->setZero();
H2->block<3,3>(0,0) = I_3x3;
H2->block<3,3>(3,3) = Ri;
H2->block<3,3>(6,6) = Ri;
}
if (p().omegaCoriolis) delta += integrateCoriolis(state_i);
return delta;
}
@ -299,11 +345,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
Ri.transpose(); // dfV/dVj
}
if (H5) {
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_biasCorrected_bias.block<3,3>(0,3);
const Matrix36 JbiasOmega = D_cDeltaRij_cOmega * D_biasCorrected_bias.middleRows<3>(0);
(*H5) <<
Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega), // dfR/dBias
-delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias
-delVdelBiasAcc(), -delVdelBiasOmega(); // dfV/dBias
-D_fR_fRrot * fRrot.inverse().matrix() * JbiasOmega, // dfR/dBias
-D_biasCorrected_bias.middleRows<3>(3), // dfP/dBias
-D_biasCorrected_bias.middleRows<3>(6); // dfV/dBias
}
// TODO(frank): Vector9 r = state_i.localCoordinates(predictedState_j); does not work ???
Vector9 r;

10
gtsam/navigation/PreintegrationBase.h
View File

@ -63,10 +63,13 @@ public:
static Eigen::Block<Vector9,3,1> dR(Vector9& v) { return v.segment<3>(0); }
static Eigen::Block<Vector9,3,1> dP(Vector9& v) { return v.segment<3>(3); }
static Eigen::Block<Vector9,3,1> dV(Vector9& v) { return v.segment<3>(6); }
static Eigen::Block<const Vector9,3,1> dR(const Vector9& v) { return v.segment<3>(0); }
static Eigen::Block<const Vector9,3,1> dP(const Vector9& v) { return v.segment<3>(3); }
static Eigen::Block<const Vector9,3,1> dV(const Vector9& v) { return v.segment<3>(6); }
// Specialize Retract/Local that agrees with IMUFactors
// TODO(frank): This is a very specific retract. Talk to Luca about implications.
NavState retract(Vector9& v, //
NavState retract(const Vector9& v, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
if (H1||H2) throw std::runtime_error("NavState::retract derivatives not implemented yet");
return NavState(rotation().expmap(dR(v)), translation() + Point3(dP(v)), velocity() + dV(v));
@ -213,11 +216,12 @@ class PreintegrationBase : public PreintegratedRotation {
OptionalJacobian<9, 6> H = boost::none) const;
/// Integrate coriolis correction in body frame state_i
Vector9 integrateCoriolis(const NavState& state_i) const;
Vector9 integrateCoriolis(const NavState& state_i,
OptionalJacobian<9, 9> H = boost::none) const;
/// Recombine the preintegration, gravity, and coriolis in a single NavState tangent vector
Vector9 recombinedPrediction(const NavState& state_i,
Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1 = boost::none,
const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const;
/// Predict state at time j

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

@ -200,6 +200,7 @@ Vector3 v1(Vector3(0.5, 0.0, 0.0));
Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(0.5, 0.0, 0.0));
NavState state1(x1, v1);
// Measurements
Vector3 measuredOmega(M_PI / 100, 0, 0);
@ -208,22 +209,59 @@ double deltaT = 1.0;
} // namespace common
/* ************************************************************************* */
TEST(ImuFactor, BiasCorrectedDelta) {
TEST(ImuFactor, PreintegrationBaseMethods) {
using namespace common;
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
PreintegratedImuMeasurements::MakeParams(kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
boost::make_shared<PreintegratedImuMeasurements::Params>();
p->gyroscopeCovariance = kMeasuredOmegaCovariance;
p->omegaCoriolis = Vector3(0.02, 0.03, 0.04);
p->accelerometerCovariance = kMeasuredAccCovariance;
p->integrationCovariance = kIntegrationErrorCovariance;
p->use2ndOrderIntegration = false;
p->use2ndOrderCoriolis = false;
PreintegratedImuMeasurements pim(p, bias);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Vector9 expected; // TODO(frank): taken from output. Should really verify.
expected << 0.0628318531, 0.0, 0.0, 4.905, -2.19885135, -8.20622494, 9.81, -4.13885394, -16.4774682;
Matrix96 actualH;
EXPECT(assert_equal(expected, pim.biasCorrectedDelta(bias,actualH), 1e-5));
Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, boost::none), bias);
EXPECT(assert_equal(expectedH, actualH));
{ // biasCorrectedDelta
Vector9 expected; // TODO(frank): taken from output. Should really verify.
expected << 0.0628318531, 0.0, 0.0, 4.905, -2.19885135, -8.20622494, 9.81, -4.13885394, -16.4774682;
Matrix96 actualH;
EXPECT(assert_equal(expected, pim.biasCorrectedDelta(bias, actualH), 1e-5));
Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
boost::none), bias);
EXPECT(assert_equal(expectedH, actualH));
}
{
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;
Matrix99 actualH;
EXPECT(assert_equal(expected, pim.integrateCoriolis(state1, actualH), 1e-5));
Matrix expectedH = numericalDerivative11<Vector9, NavState>(
boost::bind(&PreintegrationBase::integrateCoriolis, pim, _1,
boost::none), state1);
EXPECT(assert_equal(expectedH, actualH));
}
{
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;
Matrix99 actualH1, actualH2;
Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias);
EXPECT(
assert_equal(expected,
pim.recombinedPrediction(state1, biasCorrectedDelta, actualH1,
actualH2), 1e-5));
Matrix expectedH1 = numericalDerivative11<Vector9, NavState>(
boost::bind(&PreintegrationBase::recombinedPrediction, pim, _1,
biasCorrectedDelta, boost::none, boost::none), state1);
EXPECT(assert_equal(expectedH1, actualH1));
Matrix expectedH2 = numericalDerivative11<Vector9, Vector9>(
boost::bind(&PreintegrationBase::recombinedPrediction, pim, state1, _1,
boost::none, boost::none), biasCorrectedDelta);
EXPECT(assert_equal(expectedH2, actualH2));
}
}
/* ************************************************************************* */