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

Add unit test for checking covariance of CombinedImuFactor

release/4.3a0
Varun Agrawal 2021-09-17 13:28:42 -04:00
parent 9dbb431db9
commit 7e48962f99
2 changed files with 45 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -130,8 +130,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
const Matrix3& iCov = p().integrationCovariance; const Matrix3& iCov = p().integrationCovariance;
// first order uncertainty propagation // first order uncertainty propagation
// Optimized matrix multiplication: (1/dt) * G * measurementCovariance * // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
// G.transpose()
Eigen::Matrix<double, 15, 15> G_measCov_Gt; Eigen::Matrix<double, 15, 15> G_measCov_Gt;
G_measCov_Gt.setZero(15, 15); G_measCov_Gt.setZero(15, 15);
@ -144,9 +143,11 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
* aCov_updated * aCov_updated
* (vel_H_biasAcc.transpose()); * (vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
* (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
* (theta_H_biasOmega.transpose()); * (theta_H_biasOmega.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;

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

@ -203,6 +203,48 @@ TEST(CombinedImuFactor, PredictRotation) {
EXPECT(assert_equal(expectedPose, actual.pose(), tol)); EXPECT(assert_equal(expectedPose, actual.pose(), tol));
} }
/* ************************************************************************* */
// Testing covariance to check if all the jacobians are accounted for.
TEST(CombinedImuFactor, CheckCovariance) {
auto params = PreintegrationCombinedParams::MakeSharedU(9.81);
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
params->setIntegrationCovariance(pow(0.0, 2) * I_3x3);
params->setOmegaCoriolis(Vector3::Zero());
imuBias::ConstantBias currentBias;
PreintegratedCombinedMeasurements actual(params, currentBias);
// Measurements
Vector3 measuredAcc(0.1577, -0.8251, 9.6111);
Vector3 measuredOmega(-0.0210, 0.0311, 0.0145);
double deltaT = 0.01;
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Eigen::Matrix<double, 15, 15> expected;
expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, //
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
// regression
EXPECT(assert_equal(expected, actual.preintMeasCov()));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;