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

actually test the covariances and fix bug

release/4.3a0
Varun Agrawal 2021-09-19 21:15:51 -04:00
parent 3132cfbc86
commit 53712149f9
2 changed files with 19 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -141,11 +141,11 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
D_t_t(&G_measCov_Gt) = (pos_H_biasAcc D_t_t(&G_measCov_Gt) = (pos_H_biasAcc
* (aCov / dt) // aCov_updated / dt * (aCov / dt) // aCov_updated / dt
* pos_H_biasAcc.transpose()) + (dt * iCov); * pos_H_biasAcc.transpose()) + (dt * iCov);
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc D_v_v(&G_measCov_Gt) = vel_H_biasAcc
* (aCov / dt) // aCov_updated / dt * (aCov / dt) // aCov_updated / dt
* (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) = theta_H_biasOmega
* (wCov / dt) // wCov_updated / dt * (wCov / dt) // wCov_updated / dt
* (theta_H_biasOmega.transpose()); * (theta_H_biasOmega.transpose());

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

@ -269,27 +269,14 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
// EXPECT(assert_equal(expected, actual.preintMeasCov())); // EXPECT(assert_equal(expected, actual.preintMeasCov()));
// } // }
TEST(CombinedImuFactor, SameCovarianceCombined) { TEST(CombinedImuFactor, SameCovariance) {
auto params = PreintegrationCombinedParams::MakeSharedU();
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
params->setIntegrationCovariance(pow(0, 2) * I_3x3);
params->setOmegaCoriolis(Vector3::Zero());
imuBias::ConstantBias currentBias;
PreintegratedCombinedMeasurements pim(params, currentBias);
Vector3 accMeas(0.1577, -0.8251, 9.6111); Vector3 accMeas(0.1577, -0.8251, 9.6111);
Vector3 omegaMeas(-0.0210, 0.0311, 0.0145); Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
double deltaT = 0.01; double deltaT = 0.01;
pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
std::cout << pim.preintMeasCov() << std::endl << std::endl; imuBias::ConstantBias currentBias;
}
TEST(CombinedImuFactor, SameCovariance) {
auto params = PreintegrationParams::MakeSharedU(); auto params = PreintegrationParams::MakeSharedU();
params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
@ -297,16 +284,23 @@ TEST(CombinedImuFactor, SameCovariance) {
params->setIntegrationCovariance(pow(0, 2) * I_3x3); params->setIntegrationCovariance(pow(0, 2) * I_3x3);
params->setOmegaCoriolis(Vector3::Zero()); params->setOmegaCoriolis(Vector3::Zero());
imuBias::ConstantBias currentBias;
PreintegratedImuMeasurements pim(params, currentBias); PreintegratedImuMeasurements pim(params, currentBias);
Vector3 accMeas(0.1577, -0.8251, 9.6111);
Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
double deltaT = 0.01;
pim.integrateMeasurement(accMeas, omegaMeas, deltaT); pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
std::cout << pim.preintMeasCov() << std::endl << std::endl; // std::cout << pim.preintMeasCov() << std::endl << std::endl;
auto combined_params = PreintegrationCombinedParams::MakeSharedU();
combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
combined_params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
combined_params->setIntegrationCovariance(pow(0, 2) * I_3x3);
combined_params->setOmegaCoriolis(Vector3::Zero());
PreintegratedCombinedMeasurements cpim(combined_params, currentBias);
cpim.integrateMeasurement(accMeas, omegaMeas, deltaT);
// std::cout << cpim.preintMeasCov() << std::endl << std::endl;
EXPECT(assert_equal(pim.preintMeasCov(), cpim.preintMeasCov().block(0, 0, 9, 9)));
} }