General noise models

gtsam/navigation/AggregateImuReadings.cpp

@@ -25,8 +25,10 @@ namespace gtsam {
 AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr<Params>& p,
                                            const Bias& estimatedBias)
     : p_(p),
-      accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)),
+      accelerometerNoiseModel_(
-      gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)),
+          noiseModel::Gaussian::Covariance(p->accelerometerCovariance, true)),
+      gyroscopeNoiseModel_(
+          noiseModel::Gaussian::Covariance(p->gyroscopeCovariance, true)),
       estimatedBias_(estimatedBias),
       k_(0),
       deltaTij_(0.0) {
@@ -34,28 +36,19 @@ AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr<Params>& p,
   cov_.setZero();
 }
 
-SharedDiagonal AggregateImuReadings::discreteAccelerometerNoiseModel(
-    double dt) const {
-  return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() /
-                                      std::sqrt(dt));
-}
-
-SharedDiagonal AggregateImuReadings::discreteGyroscopeNoiseModel(
-    double dt) const {
-  return noiseModel::Diagonal::Sigmas(gyroscopeNoiseModel_->sigmas() /
-                                      std::sqrt(dt));
-}
-
 // Tangent space sugar.
 namespace sugar {
-typedef const Vector9 constV9;
+
 static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) { return v.segment<3>(0); }
 static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) { return v.segment<3>(3); }
 static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) { return v.segment<3>(6); }
+
+typedef const Vector9 constV9;
 static Eigen::Block<constV9, 3, 1> dR(constV9& v) { return v.segment<3>(0); }
 static Eigen::Block<constV9, 3, 1> dP(constV9& v) { return v.segment<3>(3); }
 static Eigen::Block<constV9, 3, 1> dV(constV9& v) { return v.segment<3>(6); }
-}
+
+}  // namespace sugar
 
 Vector9 AggregateImuReadings::UpdateEstimate(
     const Vector9& zeta, const Vector3& correctedAcc,
@@ -63,24 +56,38 @@ Vector9 AggregateImuReadings::UpdateEstimate(
     OptionalJacobian<9, 3> Ba, OptionalJacobian<9, 3> Bw) {
   using namespace sugar;
 
+  const Vector3 a_dt = correctedAcc * dt;
+  const Vector3 w_dt = correctedOmega * dt;
+
   // Calculate exact mean propagation
-  Matrix3 dexp;
+  Matrix3 D_R_theta;
-  const Rot3 R = Rot3::Expmap(dR(zeta), dexp);
+  const Rot3 R = Rot3::Expmap(dR(zeta), D_R_theta).matrix();
-  const Matrix3 F = dexp.inverse() * dt, H = R.matrix() * dt;
+  const Matrix3 invH = D_R_theta.inverse();
+
+  Matrix3 D_Radt_R, D_Radt_adt;
+  const Vector3 Radt = R.rotate(a_dt, A ? &D_Radt_R : 0, A ? &D_Radt_adt : 0);
 
   Vector9 zeta_plus;
-  dR(zeta_plus) = dR(zeta) + F * correctedOmega;
+  const double dt2 = 0.5 * dt;
-  dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + H * (0.5 * dt) * correctedAcc;
+  dR(zeta_plus) = dR(zeta) + invH * w_dt;
-  dV(zeta_plus) = dV(zeta) + H * correctedAcc;
+  dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + Radt * dt2;
+  dV(zeta_plus) = dV(zeta) + Radt;
 
   if (A) {
-    const Matrix3 Avt = skewSymmetric(-correctedAcc * dt);
+    // Exact derivative of R*a*dt with respect to theta:
+    const Matrix3 D_Radt_theta = D_Radt_R * D_R_theta;
+
+    // First order (small angle) approximation of derivative of invH*w*dt:
+    const Matrix3 D_invHwdt_theta = skewSymmetric(-0.5 * w_dt);
+
     A->setIdentity();
-    A->block<3, 3>(6, 0) = Avt;
+    A->block<3, 3>(0, 0) += D_invHwdt_theta;
+    A->block<3, 3>(3, 0) = D_Radt_theta * dt2;
     A->block<3, 3>(3, 6) = I_3x3 * dt;
+    A->block<3, 3>(6, 0) = D_Radt_theta;
   }
-  if (Ba) *Ba << Z_3x3, H*(dt * 0.5), H;
+  if (Ba) *Ba << Z_3x3, D_Radt_adt* dt* dt2, D_Radt_adt* dt;
-  if (Bw) *Bw << F, Z_3x3, Z_3x3;
+  if (Bw) *Bw << invH* dt, Z_3x3, Z_3x3;
 
   return zeta_plus;
 }

gtsam/navigation/AggregateImuReadings.h

@@ -22,16 +22,6 @@
 
 namespace gtsam {
 
-// Convert covariance to diagonal noise model, if possible, otherwise throw
-static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
-  bool smart = true;
-  auto model = noiseModel::Gaussian::Covariance(covariance, smart);
-  auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
-  if (!diagonal)
-    throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
-  return diagonal;
-}
-
 /**
  * Class that integrates state estimate on the manifold.
  * We integrate zeta = [theta, position, velocity]
@@ -44,7 +34,7 @@ class AggregateImuReadings {
 
  private:
   const boost::shared_ptr<Params> p_;
-  const SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_;
+  const SharedGaussian accelerometerNoiseModel_, gyroscopeNoiseModel_;
   const Bias estimatedBias_;
 
   size_t k_;         ///< index/count of measurements integrated
@@ -61,12 +51,6 @@ class AggregateImuReadings {
   const Vector9& zeta() const { return zeta_; }
   const Matrix9& zetaCov() const { return cov_; }
 
-  // We obtain discrete-time noise models by dividing the continuous-time
-  // covariances by dt:
-
-  SharedDiagonal discreteAccelerometerNoiseModel(double dt) const;
-  SharedDiagonal discreteGyroscopeNoiseModel(double dt) const;
-
   /**
    * Add a single IMU measurement to the preintegration.
    * @param measuredAcc Measured acceleration (in body frame)
@@ -91,9 +75,9 @@ class AggregateImuReadings {
   static Vector9 UpdateEstimate(const Vector9& zeta,
                                 const Vector3& correctedAcc,
                                 const Vector3& correctedOmega, double dt,
-                                OptionalJacobian<9, 9> A,
+                                OptionalJacobian<9, 9> A = boost::none,
-                                OptionalJacobian<9, 3> Ba,
+                                OptionalJacobian<9, 3> Ba = boost::none,
-                                OptionalJacobian<9, 3> Bw);
+                                OptionalJacobian<9, 3> Bw = boost::none);
 };
 
 }  // namespace gtsam

gtsam/navigation/ScenarioRunner.cpp

@@ -65,7 +65,7 @@ Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
   Vector9 sum = Vector9::Zero();
   for (size_t i = 0; i < N; i++) {
     auto pim = integrate(T, estimatedBias, true);
-#if 1
+#if 0
     NavState sampled = predict(pim);
     Vector9 xi = sampled.localCoordinates(prediction);
 #else

gtsam/navigation/ScenarioRunner.h

@@ -22,6 +22,16 @@
 
 namespace gtsam {
 
+// Convert covariance to diagonal noise model, if possible, otherwise throw
+static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
+  bool smart = true;
+  auto model = noiseModel::Gaussian::Covariance(covariance, smart);
+  auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
+  if (!diagonal)
+    throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
+  return diagonal;
+}
+
 /*
  *  Simple class to test navigation scenarios.
  *  Takes a trajectory scenario as input, and can generate IMU measurements

gtsam/navigation/tests/testAggregateImuReadings.cpp

@@ -178,12 +178,12 @@ TEST(AggregateImuReadings, UpdateEstimate) {
       boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt,
                   boost::none, boost::none, boost::none);
   Vector9 zeta;
-  zeta << 0.1, 0.2, 0.3, 0.1, 0.2, 0.3, 0.1, 0.2, 0.3;
+  zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
-  const Vector3 acc(0.1, 0.2, 0.3), omega(0.1, 0.2, 0.3);
+  const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
   pim.UpdateEstimate(zeta, acc, omega, kDt, aH1, aH2, aH3);
-  EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1));
+  EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
-  EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2));
+  EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-5));
-  EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3));
+  EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-5));
 }
 
 /* ************************************************************************* */

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -56,12 +56,6 @@ TEST(ScenarioRunner, Spin) {
   auto pim = runner.integrate(T);
   EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
 
-  // Check noise model agreement
-  EXPECT(assert_equal(p->accelerometerCovariance / kDt,
-                      pim.discreteAccelerometerNoiseModel(kDt)->covariance()));
-  EXPECT(assert_equal(p->gyroscopeCovariance / kDt,
-                      pim.discreteGyroscopeNoiseModel(kDt)->covariance()));
-
 #if 0
   // Check sampled noise is kosher
   Matrix6 expected;