Some debugging of zeta

gtsam/navigation/AggregateImuReadings.cpp

@@ -168,6 +168,13 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
   deltaTij_ += dt;
 }
 
+Vector9 AggregateImuReadings::zeta() const {
+  Vector9 zeta;
+  zeta << values.at<Vector3>(T(k_)), values.at<Vector3>(P(k_)),
+      values.at<Vector3>(V(k_));
+  return zeta;
+}
+
 NavState AggregateImuReadings::predict(const NavState& state_i,
                                        const Bias& bias_i,
                                        OptionalJacobian<9, 9> H1,
@@ -179,12 +186,14 @@ NavState AggregateImuReadings::predict(const NavState& state_i,
   Vector3 pos = values.at<Vector3>(P(k_));
   Vector3 vel = values.at<Vector3>(V(k_));
 
-  // Correct for initial velocity and gravity
+// Correct for initial velocity and gravity
+#if 0
   Rot3 Ri = state_i.attitude();
   Matrix3 Rit = Ri.transpose();
   Vector3 gt = deltaTij_ * p_->n_gravity;
   pos += Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt);
   vel += Rit * gt;
+#endif
 
   // Convert local coordinates to manifold near state_i
   Vector9 zeta;

gtsam/navigation/AggregateImuReadings.h

@@ -90,6 +90,8 @@ class AggregateImuReadings {
   void integrateMeasurement(const Vector3& measuredAcc,
                             const Vector3& measuredOmega, double dt);
 
+  Vector9 zeta() const;
+
   /// Predict state at time j
   NavState predict(const NavState& state_i, const Bias& estimatedBias_i,
                    OptionalJacobian<9, 9> H1 = boost::none,

gtsam/navigation/ScenarioRunner.cpp

@@ -59,9 +59,15 @@ Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
   Matrix samples(9, N);
   Vector9 sum = Vector9::Zero();
   for (size_t i = 0; i < N; i++) {
-    NavState sampled = predict(integrate(T, estimatedBias, true));
-    samples.col(i) = sampled.localCoordinates(prediction);
-    sum += samples.col(i);
+    auto pim = integrate(T, estimatedBias, true);
+#if 0
+    NavState sampled = predict(pim);
+    Vector9 zeta = sampled.localCoordinates(prediction);
+#else
+    Vector9 xi = pim.zeta();
+#endif
+    samples.col(i) = xi;
+    sum += xi;
   }
 
   // Compute MC covariance

gtsam/navigation/ScenarioRunner.h

@@ -86,7 +86,7 @@ class ScenarioRunner {
                    const Bias& estimatedBias = Bias()) const;
 
   /// Compute a Monte Carlo estimate of the predict covariance using N samples
-  Matrix9 estimateCovariance(double T, size_t N = 1000,
+  Matrix9 estimateCovariance(double T, size_t N = 10000,
                              const Bias& estimatedBias = Bias()) const;
 
   /// Estimate covariance of sampled noise for sanity-check

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -61,17 +61,17 @@ TEST(ScenarioRunner, Spin) {
   EXPECT(assert_equal(p->gyroscopeCovariance / kDt,
                       pim.discreteGyroscopeNoiseModel(kDt)->covariance()));
 
-#ifdef SANITY_CHECK_SAMPLER
+#if 0
   // Check sampled noise is kosher
   Matrix6 expected;
   expected << p->accelerometerCovariance / kDt, Z_3x3,  //
       Z_3x3, p->gyroscopeCovariance / kDt;
-  Matrix6 actual = runner.estimateNoiseCovariance(100000);
+  Matrix6 actual = runner.estimateNoiseCovariance(10000);
   EXPECT(assert_equal(expected, actual, 1e-2));
 #endif
 
   // Check calculated covariance against Monte Carlo estimate
-  Matrix9 estimatedCov = runner.estimateCovariance(T, 1000);
+  Matrix9 estimatedCov = runner.estimateCovariance(T);
   EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
 }