39 static std::shared_ptr<PreintegratedCombinedMeasurements::Params>
Params(
40 const Matrix3& biasAccCovariance = Matrix3::Zero(),
41 const Matrix3& biasOmegaCovariance = Matrix3::Zero(),
42 const Matrix6& biasAccOmegaInt = Matrix6::Zero()) {
43 auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(
kGravity);
46 p->integrationCovariance = 0.0001 * I_3x3;
47 p->biasAccCovariance = biasAccCovariance;
48 p->biasOmegaCovariance = biasOmegaCovariance;
49 p->biasAccOmegaInt = biasAccOmegaInt;
93 p->omegaCoriolis =
Vector3(0,0.1,0.1);
116 noiseModel::Gaussian::Covariance(combined_pim.
preintMeasCov());
126 Matrix H1e, H2e, H3e, H4e, H5e;
130 Matrix H1a, H2a, H3a, H4a, H5a, H6a;
142 #ifdef GTSAM_TANGENT_PREINTEGRATION
150 return pim.preintegrated();
158 pim.preintegrated_H_biasAcc()));
160 pim.preintegrated_H_biasOmega(), 1
e-3));
173 const double deltaT = 0.01;
177 for (
int i = 0;
i < 100; ++
i)
188 const Vector3 expectedVelocity(0, 1, 0);
200 const double deltaT = 0.01;
201 const double tol = 1
e-4;
202 for (
int i = 0;
i < 100; ++
i)
217 auto params = PreintegrationCombinedParams::MakeSharedU(9.81);
219 params->setAccelerometerCovariance(
pow(0.01, 2) * I_3x3);
220 params->setGyroscopeCovariance(
pow(1.75
e-4, 2) * I_3x3);
221 params->setIntegrationCovariance(
pow(0.0, 2) * I_3x3);
222 params->setOmegaCoriolis(Vector3::Zero());
236 expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
237 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
238 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
239 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0,
240 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0,
241 0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0,
242 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0,
243 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0,
244 0, 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0,
245 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
246 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
247 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0,
248 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
249 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
250 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
261 Vector3 accMeas(0.1577, -0.8251, 9.6111);
262 Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
269 auto params = PreintegrationParams::MakeSharedU();
270 params->setAccelerometerCovariance(
pow(0.01, 2) * I_3x3);
271 params->setGyroscopeCovariance(
pow(1.75
e-4, 2) * I_3x3);
272 params->setIntegrationCovariance(
pow(0, 2) * I_3x3);
273 params->setOmegaCoriolis(Vector3::Zero());
280 auto combined_params = PreintegrationCombinedParams::MakeSharedU();
281 combined_params->setAccelerometerCovariance(
pow(0.01, 2) * I_3x3);
282 combined_params->setGyroscopeCovariance(
pow(1.75
e-4, 2) * I_3x3);
284 combined_params->setIntegrationCovariance(Z_3x3);
285 combined_params->setOmegaCoriolis(
Z_3x1);
287 combined_params->setBiasAccOmegaInit(Z_6x6);
300 const double a = 0.2,
v = 50;
305 const Point3 initial_position(10, 20, 0);
306 const Vector3 initial_velocity(
v, 0, 0);
311 const double T = 3.0;
325 const double a = 0.2,
v = 50;
330 const Point3 initial_position(10, 20, 0);
331 const Vector3 initial_velocity(
v, 0, 0);
336 const double T = 3.0;
342 0.1 * Matrix6::Identity()),
355 Matrix6 expected_Q_init = I_6x6 * 0.001;