Monte Carlo analysis

gtsam/navigation/Scenario.h

@@ -16,12 +16,13 @@
  */
 
 #pragma once
+#include <gtsam/linear/NoiseModel.h>
 #include <gtsam/geometry/Pose3.h>
 
 namespace gtsam {
 
 /**
- * Simple class with constant twist 3D trajectory.
+ * Simple IMU simulator with constant twist 3D trajectory.
  * It is also assumed that gravity is magically counteracted and has no effect
  * on trajectory. Hence, a simulated IMU yields the actual body angular
  * velocity, and negative G acceleration plus the acceleration created by the
@@ -31,8 +32,12 @@ class Scenario {
  public:
   /// Construct scenario with constant twist [w,v]
   Scenario(const Vector3& w, const Vector3& v,
-           double imuSampleTime = 1.0 / 100.0)
+           double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
-      : twist_((Vector6() << w, v).finished()), imuSampleTime_(imuSampleTime) {}
+           double accSigma = 0.01)
+      : twist_((Vector6() << w, v).finished()),
+        imuSampleTime_(imuSampleTime),
+        gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
+        accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
 
   const double& imuSampleTime() const { return imuSampleTime_; }
 
@@ -40,6 +45,17 @@ class Scenario {
   // also, uses g=10 for easy debugging
   Vector3 gravity() const { return Vector3(0, 0, -10.0); }
 
+  const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const {
+    return gyroNoiseModel_;
+  }
+
+  const noiseModel::Diagonal::shared_ptr& accNoiseModel() const {
+    return accNoiseModel_;
+  }
+
+  Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); }
+  Matrix3 accCovariance() const { return accNoiseModel_->covariance(); }
+
   Vector3 angularVelocityInBody() const { return twist_.head<3>(); }
   Vector3 linearVelocityInBody() const { return twist_.tail<3>(); }
 
@@ -76,6 +92,7 @@ class Scenario {
  private:
   Vector6 twist_;
   double imuSampleTime_;
+  noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
 };
 
 }  // namespace gtsam

gtsam/navigation/ScenarioRunner.h

@@ -16,59 +16,100 @@
  */
 
 #pragma once
+#include <gtsam/linear/Sampler.h>
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/Scenario.h>
 
-#include <iostream>
+#include <cmath>
 
 namespace gtsam {
 
-double accNoiseVar = 0.01;
+static double intNoiseVar = 0.0001;
-double omegaNoiseVar = 0.03;
+static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
-double intNoiseVar = 0.0001;
-const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
-const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3;
-const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
 
 /// Simple class to test navigation scenarios
 class ScenarioRunner {
  public:
   ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {}
 
-  // Integrate measurements for T seconds
+  /// Integrate measurements for T seconds into a PIM
-  ImuFactor::PreintegratedMeasurements integrate(double T) {
+  ImuFactor::PreintegratedMeasurements integrate(
+      double T, boost::optional<Sampler&> gyroSampler = boost::none,
+      boost::optional<Sampler&> accSampler = boost::none) {
     // TODO(frank): allow non-zero
     const imuBias::ConstantBias zeroBias;
     const bool use2ndOrderCoriolis = true;
 
-    ImuFactor::PreintegratedMeasurements result(
+    ImuFactor::PreintegratedMeasurements pim(
-        zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
+        zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(),
         kIntegrationErrorCovariance, use2ndOrderCoriolis);
 
-    const Vector3 measuredOmega = scenario_.angularVelocityInBody();
+    const double dt = scenario_.imuSampleTime();
-    const double deltaT = scenario_.imuSampleTime();
+    const double sqrt_dt = std::sqrt(dt);
-    const size_t nrSteps = T / deltaT;
+    const size_t nrSteps = T / dt;
     double t = 0;
-    for (size_t k = 0; k < nrSteps; k++, t += deltaT) {
+    for (size_t k = 0; k < nrSteps; k++, t += dt) {
-      const Vector3 measuredAcc = scenario_.accelerationInBody(t);
+      Vector3 measuredOmega = scenario_.angularVelocityInBody();
-      result.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+      if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
+      Vector3 measuredAcc = scenario_.accelerationInBody(t);
+      if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
+      pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
     }
 
-    return result;
+    return pim;
   }
 
-  // Predict mean
+  /// Predict predict given a PIM
-  Pose3 mean(const ImuFactor::PreintegratedMeasurements& integrated) {
+  PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim) {
-    // TODO(frank): allow non-standard
+    // TODO(frank): allow non-zero bias, omegaCoriolis
     const imuBias::ConstantBias zeroBias;
     const Pose3 pose_i = Pose3::identity();
     const Vector3 vel_i = scenario_.velocity(0);
     const Vector3 omegaCoriolis = Vector3::Zero();
     const bool use2ndOrderCoriolis = true;
-    const PoseVelocityBias prediction =
+    return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
-        integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
+                       omegaCoriolis, use2ndOrderCoriolis);
-                           omegaCoriolis, use2ndOrderCoriolis);
+  }
-    return prediction.pose;
+
+  /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
+  Matrix6 poseCovariance(const ImuFactor::PreintegratedMeasurements& pim) {
+    Matrix9 cov = pim.preintMeasCov();  // _ position rotation
+    Matrix6 poseCov;
+    poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3),  //
+        cov.block<3, 3>(3, 6), cov.block<3, 3>(3, 3);
+    return poseCov;
+  }
+
+  /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples
+  Matrix6 estimatePoseCovariance(double T, size_t N = 1000) {
+    // Get predict prediction from ground truth measurements
+    Pose3 prediction = predict(integrate(T)).pose;
+
+    // Create two samplers for acceleration and omega noise
+    Sampler gyroSampler(scenario_.gyroNoiseModel(), 29285);
+    Sampler accSampler(scenario_.accNoiseModel(), 29284);
+
+    // Sample !
+    Matrix samples(9, N);
+    Vector6 sum = Vector6::Zero();
+    for (size_t i = 0; i < N; i++) {
+      Pose3 sampled = predict(integrate(T, gyroSampler, accSampler)).pose;
+      Vector6 xi = sampled.localCoordinates(prediction);
+      samples.col(i) = xi;
+      sum += xi;
+    }
+
+    // Compute MC covariance
+    Vector6 sampleMean = sum / N;
+    Matrix6 Q;
+    Q.setZero();
+    for (size_t i = 0; i < N; i++) {
+      Vector6 xi = samples.col(i);
+      xi -= sampleMean;
+      Q += xi * (xi.transpose() / (N - 1));
+    }
+
+    return Q;
   }
 
  private:
@@ -76,4 +117,3 @@ class ScenarioRunner {
 };
 
 }  // namespace gtsam
-

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -27,12 +27,15 @@ static const double degree = M_PI / 180.0;
 /* ************************************************************************* */
 TEST(ScenarioRunner, Forward) {
   const double v = 2;  // m/s
-  Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
+  Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.1, 0.00001);
 
   ScenarioRunner runner(forward);
   const double T = 1;  // seconds
-  ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
+  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(forward.pose(T), runner.mean(integrated), 1e-9));
+  EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9));
+
+  Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
+  EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-9));
 }
 
 /* ************************************************************************* */
@@ -43,8 +46,9 @@ TEST(ScenarioRunner, Circle) {
 
   ScenarioRunner runner(circle);
   const double T = 15;  // seconds
-  ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
+
-  EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1));
+  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
+  EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1));
 }
 
 /* ************************************************************************* */
@@ -56,8 +60,8 @@ TEST(ScenarioRunner, Loop) {
 
   ScenarioRunner runner(loop);
   const double T = 30;  // seconds
-  ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
+  ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
-  EXPECT(assert_equal(loop.pose(T), runner.mean(integrated), 0.1));
+  EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1));
 }
 
 /* ************************************************************************* */