Fixed covariances by dividing by dt or dt22, so the right-hand nosiy measurement is indeed used with the correct noise model

gtsam/navigation/ScenarioRunner.cpp

@@ -76,21 +76,21 @@ PreintegratedMeasurements2::initPosterior(const Vector3& correctedAcc,
   GaussianFactorGraph graph;
 
   // theta(1) = (correctedOmega - bias_delta) * dt
-  // => theta(1) + bias_delta * dt = correctedOmega * dt
+  // => theta(1)/dt + bias_delta = correctedOmega
-  graph.add<Terms>({{T(k_ + 1), I_3x3}, {kBiasKey, omega_H_bias * dt}},
+  auto I_dt = I_3x3 / dt;
-                   correctedOmega * dt, discreteGyroscopeNoiseModel(dt));
+  graph.add<Terms>({{T(k_ + 1), I_dt}, {kBiasKey, omega_H_bias}},
+                   correctedOmega, discreteGyroscopeNoiseModel(dt));
 
-  // pose(1) = (correctedAcc - bias_delta) * dt^2/2
+  // pose(1) = (correctedAcc - bias_delta) * dt22
-  // => pose(1) + bias_delta * dt^2/2 = correctedAcc * dt^2/2
+  // => pose(1) / dt22 + bias_delta = correctedAcc
-  double dt22 = 0.5 * dt * dt;
   auto accModel = discreteAccelerometerNoiseModel(dt);
-  graph.add<Terms>({{P(k_ + 1), I_3x3}, {kBiasKey, acc_H_bias * dt22}},
+  graph.add<Terms>({{P(k_ + 1), I_dt * (2.0 / dt)}, {kBiasKey, acc_H_bias}},
-                   correctedAcc * dt22, accModel);
+                   correctedAcc, accModel);
 
   // vel(1) = (correctedAcc - bias_delta) * dt
-  // => vel(1) + bias_delta * dt = correctedAcc * dt
+  // => vel(1)/dt + bias_delta = correctedAcc
-  graph.add<Terms>({{V(k_ + 1), I_3x3}, {kBiasKey, acc_H_bias * dt}},
+  graph.add<Terms>({{V(k_ + 1), I_dt}, {kBiasKey, acc_H_bias}}, correctedAcc,
-                   correctedAcc * dt, accModel);
+                   accModel);
 
   // eliminate all but biases
   // NOTE(frank): After this, posterior_k_ contains P(zeta(1)|bias)
@@ -116,27 +116,27 @@ PreintegratedMeasurements2::integrateCorrected(const Vector3& correctedAcc,
     graph.add(boost::static_pointer_cast<GaussianFactor>(conditional));
 
   // theta(k+1) = theta(k) + inverse(H)*(correctedOmega - bias_delta) dt
-  // =>  H*theta(k+1) - H*theta(k) + bias_delta dt = (measuredOmega - bias) dt
+  // =>  H*theta(k+1)/dt - H*theta(k)/dt + bias_delta = (measuredOmega - bias)
-  Matrix3 H = Rot3::ExpmapDerivative(theta_k);
+  Matrix3 H = Rot3::ExpmapDerivative(theta_k) / dt;
-  graph.add<Terms>({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_H_bias * dt}},
+  graph.add<Terms>({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_H_bias}},
-                   correctedOmega * dt, discreteGyroscopeNoiseModel(dt));
+                   correctedOmega, discreteGyroscopeNoiseModel(dt));
 
-  // pos(k+1) = pos(k) + vel(k) dt + Rk*(correctedAcc - bias_delta) dt^2/2
-  //  => Rkt*pos(k+1) - Rkt*pos(k) - Rkt*vel(k) dt + bias_delta dt^2/2
-  //     = correctedAcc dt^2/2
   double dt22 = 0.5 * dt * dt;
+  // pos(k+1) = pos(k) + vel(k) dt + Rk*(correctedAcc - bias_delta) dt22
+  //  => Rkt*pos(k+1)/dt22 - Rkt*pos(k)/dt22 - Rkt*vel(k) dt/dt22 + bias_delta
+  //     = correctedAcc
   auto accModel = discreteAccelerometerNoiseModel(dt);
-  graph.add<Terms>({{P(k_ + 1), Rkt},
+  graph.add<Terms>({{P(k_ + 1), Rkt / dt22},
-                    {P(k_), -Rkt},
+                    {P(k_), -Rkt / dt22},
-                    {V(k_), -Rkt * dt},
+                    {V(k_), -Rkt * (2.0 / dt)},
-                    {kBiasKey, acc_H_bias * dt22}},
+                    {kBiasKey, acc_H_bias}},
-                   correctedAcc * dt22, accModel);
+                   correctedAcc, accModel);
 
   // vel(k+1) = vel(k) + Rk*(correctedAcc - bias_delta) dt
-  // =>  Rkt*vel(k+1) - Rkt*vel(k) + bias_delta dt = correctedAcc * dt
+  // =>  Rkt*vel(k+1)/dt - Rkt*vel(k)/dt + bias_delta = correctedAcc
   graph.add<Terms>(
-      {{V(k_ + 1), Rkt}, {V(k_), -Rkt}, {kBiasKey, acc_H_bias * dt}},
+      {{V(k_ + 1), Rkt / dt}, {V(k_), -Rkt / dt}, {kBiasKey, acc_H_bias}},
-      correctedAcc * dt, accModel);
+      correctedAcc, accModel);
 
   // eliminate all but biases
   // TODO(frank): does not seem to eliminate in order I want. What gives?
@@ -257,8 +257,7 @@ Matrix9 ScenarioRunner::estimateCovariance(
   Matrix9 Q;
   Q.setZero();
   for (size_t i = 0; i < N; i++) {
-    Vector9 xi = samples.col(i);
+    Vector9 xi = samples.col(i) - sampleMean;
-    xi -= sampleMean;
     Q += xi * xi.transpose();
   }
 
@@ -279,8 +278,7 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
   Matrix6 Q;
   Q.setZero();
   for (size_t i = 0; i < N; i++) {
-    Vector6 xi = samples.col(i);
+    Vector6 xi = samples.col(i) - sampleMean;
-    xi -= sampleMean;
     Q += xi * xi.transpose();
   }
 

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -48,7 +48,7 @@ TEST(ScenarioRunner, Spin) {
 
   auto p = defaultParams();
   ScenarioRunner runner(&scenario, p, kDt);
-  const double T = kDt;  // seconds
+  const double T = 2 * kDt;  // seconds
 
   auto pim = runner.integrate(T);
   EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
@@ -59,15 +59,17 @@ TEST(ScenarioRunner, Spin) {
   EXPECT(assert_equal(p->gyroscopeCovariance / kDt,
                       pim.discreteGyroscopeNoiseModel(kDt)->covariance()));
 
+#ifdef SANITY_CHECK_SAMPLER
   // Check sampled noise is kosher
   Matrix6 expected;
   expected << p->accelerometerCovariance / kDt, Z_3x3,  //
       Z_3x3, p->gyroscopeCovariance / kDt;
-  Matrix6 actual = runner.estimateNoiseCovariance(10000);
+  Matrix6 actual = runner.estimateNoiseCovariance(100000);
   EXPECT(assert_equal(expected, actual, 1e-2));
+#endif
 
   // Check calculated covariance against Monte Carlo estimate
-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 1000);
   EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
 }