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

tested Jacobians computation (G) for combined IMU factor

release/4.3a0
Luca 2014-12-09 15:50:41 -05:00
parent 53b59bf488
commit 2c1d72e7d7
2 changed files with 36 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -143,9 +143,9 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
(measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) *
(H_angles_biasomega.transpose());
G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9);
G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * measurementCovariance_.block<3,3>(9,9);
G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12);
G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * measurementCovariance_.block<3,3>(12,12);
// NEW OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose();
@ -164,10 +164,10 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// This is for testing & documentation
///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3,
Z_3x3, Z_3x3, H_angles_angles, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3;
Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3,
Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
}
}

49
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -382,25 +382,36 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
EXPECT(assert_equal(Fexpected, Factual));
//
// // Compute expected G wrt integration noise
// Matrix df_dintNoise(9,3);
// df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3;
//
// // Compute expected F wrt acc noise
// Matrix df_daccNoise =
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
// deltaPij_old, deltaVij_old, logDeltaRij_old,
// _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
// // Compute expected F wrt gyro noise
// Matrix df_domegaNoise =
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
// deltaPij_old, deltaVij_old, logDeltaRij_old,
// newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
// Matrix Gexpected(9,9);
//
// Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise;
// EXPECT(assert_equal(Gexpected, Gactual));
// Compute expected G wrt integration noise
Matrix df_dintNoise(15,3);
df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3;
// Compute expected F wrt acc noise (15,3)
Matrix df_daccNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, logDeltaRij_old, bias_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise (15,3)
Matrix df_domegaNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, logDeltaRij_old, bias_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
// Compute expected F wrt bias random walk noise (15,6)
Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries
df_rwBias.setZero();
df_rwBias.block<6,6>(9,0) = eye(6);
// Compute expected F wrt gyro noise (15,3)
Matrix df_dinitBias =
numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, logDeltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows
Matrix Gexpected(15,21);
Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;
EXPECT(assert_equal(Gexpected, Gactual));
}
/* ************************************************************************* */