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

more Monte Carlo

release/4.3a0
dellaert 2015-08-01 17:21:34 -07:00
parent b8f05e1e35
commit 18d0966630
1 changed files with 42 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -117,14 +117,26 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
const NavState& state, const imuBias::ConstantBias& bias, double dt,
const Pose3& body_P_sensor, const Vector3& measuredAcc,
const Vector3& measuredOmega, size_t N = 10000) {
const Vector3& measuredOmega, size_t N = 10, size_t M = 1) {
// Get mean prediction from "ground truth" measurements
PreintegratedImuMeasurements pim1 = pim;
for (size_t k=0;k<10;k++)
for (size_t k=0;k<M;k++)
pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
NavState mean = pim1.predict(state, bias);
NavState prediction = pim1.predict(state, bias);
// NavState prediction = pim1.deltaXij();
GTSAM_PRINT(prediction);
cout << "pim1.preintMeasCov():" << endl;
cout << pim1.preintMeasCov() << endl;
cout << 1000000*pim1.preintMeasCov() << endl;
// Below we will call xi = sampled.localCoordinates(prediction) to get a vector, which is
// h = between(g); // derivatives inlined below
// xi = Local(h, D_v_h);
// with derivatives
// D_xi_sampled = - D_v_h * h.inverse().AdjointMap();
// D_xi_predict = D_v_h;
// But with h near identity they are
// D_xi_sampled = - I_3x3;
// D_xi_predict = I_3x3;
// Do a Monte Carlo analysis to determine empirical density on the predicted state
Sampler sampleAccelerationNoise(Vector3::Constant(sqrt(accNoiseVar/dt)));
@ -133,14 +145,18 @@ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
Vector9 sum = Vector9::Zero();
for (size_t i = 0; i < N; i++) {
PreintegratedImuMeasurements pim2 = pim;
for (size_t k=0;k<10;k++) {
for (size_t k=0;k<M;k++) {
Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample();
Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample();
pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, body_P_sensor);
}
NavState prediction = pim2.predict(state, bias);
samples.col(i) = mean.localCoordinates(prediction);
sum += samples.col(i);
NavState sampled = pim2.predict(state, bias);
// NavState sampled = pim2.deltaXij();
Vector9 xi = sampled.localCoordinates(prediction);
GTSAM_PRINT(sampled);
cout << xi.transpose() << endl;
samples.col(i) = xi;
sum += xi;
}
Vector9 sampleMean = sum / N;
cout << ":" << endl;
@ -149,13 +165,14 @@ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
Q.setZero();
for (size_t i = 0; i < N; i++) {
Vector9 xi = samples.col(i);
#define SUBTRACT_SAMPLE_MEAN
#ifdef SUBTRACT_SAMPLE_MEAN
xi -= sampleMean;
#endif
Q += xi * xi.transpose() / (N - 1);
}
cout << "Q:" << endl;
cout << Q << endl;
cout << 1000000*Q << endl;
return Q;
}
@ -163,7 +180,7 @@ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
TEST(ImuFactor, StraightLine) {
// Set up IMU measurements
static const double g = 10; // make gravity 10 to make this easy to check
static const double v = 5.0, a = 0.2, dt = 3.0;
static const double v = 50.0, a = 0.2, dt = 3.0;
const double dt22 = dt * dt / 2;
// Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
@ -183,12 +200,26 @@ TEST(ImuFactor, StraightLine) {
p->accelerometerCovariance = kMeasuredAccCovariance;
p->gyroscopeCovariance = kMeasuredOmegaCovariance;
// Check G1 and G2 derivatives of pim.update
// Now, preintegrate for 3 seconds, in 10 steps
PreintegratedImuMeasurements pim(p, kZeroBiasHat);
for (size_t i = 0; i < 10; i++)
pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10);
// Get mean prediction from "ground truth" measurements
Matrix93 aG1,aG2;
boost::function<NavState(const Vector3&, const Vector3&)> f =
boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10,
boost::none, boost::none, boost::none);
pim.updatedDeltaXij(measuredAcc, measuredOmega, dt / 10, boost::none, aG1, aG2);
EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7));
cout << "aG1" << endl;
cout << aG1 << endl;
cout << "aG2:" << endl;
cout << aG2 << endl;
// Do Monte-Carlo analysis
PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance,
p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise
Matrix9 Q = MonteCarlo(pimMC, state1, kZeroBias, dt/10, Pose3(), measuredAcc,