From 21a63d8d0ee24972f35ac258ed6b3093e2927de4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 3 Jan 2016 21:46:15 -0800
Subject: [PATCH] Using covariances again

---
 gtsam/navigation/AggregateImuReadings.cpp | 21 ++++++++-------------
 gtsam/navigation/AggregateImuReadings.h   |  1 -
 2 files changed, 8 insertions(+), 14 deletions(-)

diff --git a/gtsam/navigation/AggregateImuReadings.cpp b/gtsam/navigation/AggregateImuReadings.cpp
index 07b8f8ce8..fb8178598 100644
--- a/gtsam/navigation/AggregateImuReadings.cpp
+++ b/gtsam/navigation/AggregateImuReadings.cpp
@@ -25,14 +25,7 @@ namespace gtsam {
 
 AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr<Params>& p,
                                            const Bias& estimatedBias)
-    : p_(p),
-      accelerometerNoiseModel_(
-          noiseModel::Gaussian::Covariance(p->accelerometerCovariance, true)),
-      gyroscopeNoiseModel_(
-          noiseModel::Gaussian::Covariance(p->gyroscopeCovariance, true)),
-      estimatedBias_(estimatedBias),
-      k_(0),
-      deltaTij_(0.0) {
+    : p_(p), estimatedBias_(estimatedBias), k_(0), deltaTij_(0.0) {
   zeta_.setZero();
   cov_.setZero();
 }
@@ -120,11 +113,13 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
                          useExactDexpDerivative, A, Ba, Bw);
 
   // propagate uncertainty
-  // TODO(frank): use noiseModel power: covariance is very expensive !
-  const Matrix3 w = gyroscopeNoiseModel_->covariance() / dt;
-  const Matrix3 a = accelerometerNoiseModel_->covariance() / dt;
-  cov_ = A * cov_ * A.transpose() + Bw * w * Bw.transpose() +
-         Ba * a * Ba.transpose();
+  // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
+  const Matrix3& w = p_->gyroscopeCovariance;
+  const Matrix3& a = p_->accelerometerCovariance;
+  // TODO(frank): use Eigen-tricks for symmetric matrices
+  cov_ = A * cov_ * A.transpose();
+  cov_ += Bw * (w / dt) * Bw.transpose();
+  cov_ += Ba * (a / dt) * Ba.transpose();
 
   // increment counter and time
   k_ += 1;
diff --git a/gtsam/navigation/AggregateImuReadings.h b/gtsam/navigation/AggregateImuReadings.h
index 094cc8396..225fc5eb8 100644
--- a/gtsam/navigation/AggregateImuReadings.h
+++ b/gtsam/navigation/AggregateImuReadings.h
@@ -34,7 +34,6 @@ class AggregateImuReadings {
 
  private:
   const boost::shared_ptr<Params> p_;
-  const SharedGaussian accelerometerNoiseModel_, gyroscopeNoiseModel_;
   const Bias estimatedBias_;
 
   size_t k_;         ///< index/count of measurements integrated