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

Fixed derivative of biasCorrectedDelta

release/4.3a0
dellaert 2015-07-24 17:40:35 +02:00
parent 7ebcb4c18f
commit 670781231c
2 changed files with 31 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/navigation/PreintegrationBase.cpp
View File

@ -126,10 +126,13 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// Correct deltaRij, derivative is delRdelBiasOmega_
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Rot3 deltaRij = biascorrectedDeltaRij(biasIncr.gyroscope());
Matrix3 D_deltaRij_bias;
Rot3 deltaRij = biascorrectedDeltaRij(biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0);
Vector9 xi;
Matrix3 D_dR_deltaRij;
// TODO(frank): could line below be simplified? It is equivalent to
// LogMap(deltaRij_.compose(Expmap(delRdelBiasOmega_ * biasIncr.gyroscope())))
NavState::dR(xi) = Rot3::Logmap(deltaRij, H ? &D_dR_deltaRij : 0);
NavState::dP(xi) = deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer()
+ delPdelBiasOmega_ * biasIncr.gyroscope();
@ -138,7 +141,7 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
if (H) {
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
D_dR_bias << Z_3x3, D_dR_deltaRij * delRdelBiasOmega_;
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;

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

@ -269,11 +269,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
Matrix eH1 = numericalDerivative11<NavState, NavState>(
boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none,
boost::none), state1);
EXPECT(assert_equal(eH1, aH1, 1e-8));
EXPECT(assert_equal(eH1, aH1));
Matrix eH2 = numericalDerivative11<NavState, imuBias::ConstantBias>(
boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
boost::none), bias);
EXPECT(assert_equal(eH2, aH2, 1e-8));
EXPECT(assert_equal(eH2, aH2));
return;
}
@ -329,8 +329,8 @@ TEST(ImuFactor, ErrorAndJacobians) {
expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901;
EXPECT(
assert_equal(expectedError,
factor.evaluateError(x1, v1, x2, v2_wrong, bias),1e-2));
EXPECT(assert_equal(expectedError, factor.unwhitenedError(values)));
factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-2));
EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2));
// Make sure the whitening is done correctly
Matrix cov = pim.preintMeasCov();
@ -363,6 +363,22 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Make sure of biascorrectedDeltaRij with this example...
Matrix3 aH;
pim.biascorrectedDeltaRij(bias.gyroscope(), aH);
Matrix3 eH = numericalDerivative11<Rot3, Vector3>(
boost::bind(&PreintegrationBase::biascorrectedDeltaRij, pim, _1,
boost::none), bias.gyroscope());
EXPECT(assert_equal(eH, aH));
// ... and of biasCorrectedDelta
Matrix96 actualH;
pim.biasCorrectedDelta(bias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
boost::none), bias);
EXPECT(assert_equal(expectedH, actualH));
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis);
@ -376,7 +392,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
// Make sure linearization is correct
double diffDelta = 1e-5;
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-1);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
}
/* ************************************************************************* */
@ -416,7 +432,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
// Make sure linearization is correct
double diffDelta = 1e-5;
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
}
/* ************************************************************************* */
@ -439,8 +455,7 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
// Compare Jacobians
// 1e-3 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9));
}
/* ************************************************************************* */
@ -552,8 +567,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega()));
}
/* ************************************************************************* */
@ -829,7 +843,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// Make sure linearization is correct
double diffDelta = 1e-5;
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-1);
// This fails, except if tol = 1e-1: probably wrong!
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
}
/* ************************************************************************* */