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

biasCorrectedDelta and test of its derivatives

release/4.3a0
dellaert 2015-07-19 21:24:38 -07:00
parent 1229ca6fee
commit 76abf553b0
3 changed files with 73 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
gtsam/navigation/PreintegrationBase.cpp
View File

@ -129,6 +129,31 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
} }
} }
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_deltaRij_bias;
Rot3 deltaRij = PreintegratedRotation::biascorrectedDeltaRij(
biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0);
Vector9 delta;
Matrix3 D_dR_deltaRij;
NavState::dR(delta) = Rot3::Logmap(deltaRij, H ? &D_dR_deltaRij : 0);
NavState::dP(delta) = deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer()
+ delPdelBiasOmega_ * biasIncr.gyroscope();
NavState::dV(delta) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer()
+ delVdelBiasOmega_ * biasIncr.gyroscope();
if (H) {
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
D_dR_bias << Z_3x3, D_dR_deltaRij * D_deltaRij_bias;
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
}
return delta;
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const { Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const {
Vector9 result = Vector9::Zero(); Vector9 result = Vector9::Zero();
@ -154,9 +179,8 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i, Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
const Rot3& deltaRij_biascorrected, Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1,
const Vector3& deltaPij_biascorrected, OptionalJacobian<9, 9> H2) const {
const Vector3& deltaVij_biascorrected) const {
const Pose3& pose_i = state_i.pose(); const Pose3& pose_i = state_i.pose();
const Vector3& vel_i = state_i.velocity(); const Vector3& vel_i = state_i.velocity();
@ -165,9 +189,9 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
// Rotation, translation, and velocity: // Rotation, translation, and velocity:
Vector9 delta; Vector9 delta;
NavState::dR(delta) = Rot3::Logmap(deltaRij_biascorrected); NavState::dR(delta) = NavState::dR(biasCorrectedDelta);
NavState::dP(delta) = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * p().gravity * dt2; NavState::dP(delta) = Ri * NavState::dP(biasCorrectedDelta) + vel_i * dt + 0.5 * p().gravity * dt2;
NavState::dV(delta) = Ri * deltaVij_biascorrected + p().gravity * dt; NavState::dV(delta) = Ri * NavState::dV(biasCorrectedDelta) + p().gravity * dt;
if (p().omegaCoriolis) delta += integrateCoriolis(state_i); if (p().omegaCoriolis) delta += integrateCoriolis(state_i);
return delta; return delta;
@ -175,24 +199,15 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i, NavState PreintegrationBase::predict(const NavState& state_i,
const Rot3& deltaRij_biascorrected, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
const Vector3& deltaPij_biascorrected, OptionalJacobian<9, 6> H2) const {
const Vector3& deltaVij_biascorrected) const { Vector9 biasCorrected = biasCorrectedDelta(bias_i, H2);
Matrix9 D_delta_biasCorrected;
Vector9 delta = recombinedPrediction(state_i, deltaRij_biascorrected, Vector9 delta = recombinedPrediction(state_i, biasCorrected, H1, H2 ? &D_delta_biasCorrected : 0);
deltaPij_biascorrected, deltaVij_biascorrected); if (H2) *H2 = D_delta_biasCorrected * (*H2);
return state_i.retract(delta); return state_i.retract(delta);
} }
//------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i) const {
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
return predict(state_i, biascorrectedDeltaRij(biasIncr.gyroscope()),
biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr));
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j, const Pose3& pose_j, const Vector3& vel_j,
@ -210,18 +225,15 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
const Rot3& rot_j = pose_j.rotation(); const Rot3& rot_j = pose_j.rotation();
const Vector3 pos_j = pose_j.translation().vector(); const Vector3 pos_j = pose_j.translation().vector();
/// Compute bias-corrected quantities // Compute bias-corrected quantities
const imuBias::ConstantBias biasIncr = bias_i - biasHat_; // TODO(frank): now redundant with biasCorrected below
Matrix3 D_biascorrected_biasIncr; Matrix96 D_biasCorrected_bias;
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij( Vector9 biasCorrected = biasCorrectedDelta(bias_i, D_biasCorrected_bias);
biasIncr.gyroscope(), H5 ? &D_biascorrected_biasIncr : 0);
const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr);
const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr);
/// Predict state at time j /// Predict state at time j
NavState state_i(pose_i, vel_i); NavState state_i(pose_i, vel_i);
NavState predictedState_j = predict(state_i, deltaRij_biascorrected, Vector9 delta = recombinedPrediction(state_i, biasCorrected);
deltaPij_biascorrected, deltaVij_biascorrected); NavState predictedState_j = state_i.retract(delta);
// Evaluate residual error, according to [3] // Evaluate residual error, according to [3]
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
@ -233,17 +245,10 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
// fR will be computed later. // fR will be computed later.
// Note: it is the same as: fR = predictedState_j.pose.rotation().between(Rot_j) // Note: it is the same as: fR = predictedState_j.pose.rotation().between(Rot_j)
/* ---------------------------------------------------------------------------------------------------- */
// Get Get so<3> version of bias corrected rotation
// If H5 is asked for, we will need the Jacobian, which we store in H5
// H5 will then be corrected below to take into account the Coriolis effect
Matrix3 D_omega_biascorrected;
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, H5 ? &D_omega_biascorrected : 0);
// Coriolis term, NOTE inconsistent with AHRS, where coriolisHat is *after* integration // Coriolis term, NOTE inconsistent with AHRS, where coriolisHat is *after* integration
// TODO(frank): move derivatives to predict and do coriolis branching there // TODO(frank): move derivatives to predict and do coriolis branching there
const Vector3 coriolis = PreintegratedRotation::integrateCoriolis(rot_i); const Vector3 coriolis = PreintegratedRotation::integrateCoriolis(rot_i);
const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Vector3 correctedOmega = NavState::dR(biasCorrected) - coriolis;
// Residual rotation error // Residual rotation error
Matrix3 D_cDeltaRij_cOmega; Matrix3 D_cDeltaRij_cOmega;
@ -270,9 +275,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
(*H1) << (*H1) <<
D_fR_fRrot * (-rot_j.between(rot_i).matrix() - fRrot.inverse().matrix() * D_coriolis), // dfR/dRi D_fR_fRrot * (-rot_j.between(rot_i).matrix() - fRrot.inverse().matrix() * D_coriolis), // dfR/dRi
Z_3x3, // dfR/dPi Z_3x3, // dfR/dPi
skewSymmetric(fp + deltaPij_biascorrected), // dfP/dRi skewSymmetric(fp + NavState::dP(biasCorrected)), // dfP/dRi
dfPdPi, // dfP/dPi dfPdPi, // dfP/dPi
skewSymmetric(fv + deltaVij_biascorrected), // dfV/dRi skewSymmetric(fv + NavState::dV(biasCorrected)), // dfV/dRi
dfVdPi; // dfV/dPi dfVdPi; // dfV/dPi
} }
if (H2) { if (H2) {
@ -294,8 +299,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
Ri.transpose(); // dfV/dVj Ri.transpose(); // dfV/dVj
} }
if (H5) { if (H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_biasCorrected_bias.block<3,3>(0,3);
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_omega_biascorrected * D_biascorrected_biasIncr;
(*H5) << (*H5) <<
Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega), // dfR/dBias Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega), // dfR/dBias
-delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias -delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias

26
gtsam/navigation/PreintegrationBase.h
View File

@ -207,32 +207,22 @@ class PreintegrationBase : public PreintegratedRotation {
Vector3* correctedAcc, Vector3* correctedAcc,
Vector3* correctedOmega); Vector3* correctedOmega);
Vector3 biascorrectedDeltaPij(const imuBias::ConstantBias& biasIncr) const { /// Given the estimate of the bias, return a NavState tangent vector
return deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer() /// summarizing the preintegrated IMU measurements so far
+ delPdelBiasOmega_ * biasIncr.gyroscope(); Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
} OptionalJacobian<9, 6> H = boost::none) const;
Vector3 biascorrectedDeltaVij(const imuBias::ConstantBias& biasIncr) const {
return deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer()
+ delVdelBiasOmega_ * biasIncr.gyroscope();
}
/// Integrate coriolis correction in body frame state_i /// Integrate coriolis correction in body frame state_i
Vector9 integrateCoriolis(const NavState& state_i) const; Vector9 integrateCoriolis(const NavState& state_i) const;
/// Recombine the preintegration, gravity, and coriolis in a single NavState tangent vector /// Recombine the preintegration, gravity, and coriolis in a single NavState tangent vector
Vector9 recombinedPrediction(const NavState& state_i, Vector9 recombinedPrediction(const NavState& state_i,
const Rot3& deltaRij_biascorrected, const Vector3& deltaPij_biascorrected, Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1 = boost::none,
const Vector3& deltaVij_biascorrected) const; OptionalJacobian<9, 9> H2 = boost::none) const;
/// Predict state at time j, with bias-corrected quantities given
NavState predict(const NavState& navState, const Rot3& deltaRij_biascorrected,
const Vector3& deltaPij_biascorrected,
const Vector3& deltaVij_biascorrected) const;
/// Predict state at time j /// Predict state at time j
NavState predict(const NavState& state_i, NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
const imuBias::ConstantBias& bias_i) const; OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const;
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,

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

@ -207,6 +207,25 @@ Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector();
double deltaT = 1.0; double deltaT = 1.0;
} // namespace common } // namespace common
/* ************************************************************************* */
TEST(ImuFactor, BiasCorrectedDelta) {
using namespace common;
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
PreintegratedImuMeasurements::MakeParams(kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
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));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, ErrorAndJacobians) { TEST(ImuFactor, ErrorAndJacobians) {
using namespace common; using namespace common;