Fixing the EKF example and change linearization point initialization

examples/easyPoint2KalmanFilter.cpp

@@ -100,7 +100,7 @@ int main() {
   Symbol x2('x',2);
   difference = Point2(1,0);
   BetweenFactor<Point2> factor3(x1, x2, difference, Q);
-  Point2 x2_predict = ekf.predict(factor1);
+  Point2 x2_predict = ekf.predict(factor3);
   traits<Point2>::Print(x2_predict, "X2 Predict");
 
   // Update

gtsam/nonlinear/ExtendedKalmanFilter-inl.h

@@ -64,11 +64,12 @@ namespace gtsam {
   template <class VALUE>
   ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(
       Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
-      : x_(x_initial)  // Set the initial linearization point
+      : x_(x_initial), lastLinearization(nullptr)  // Set the initial linearization point
   {
     // Create a Jacobian Prior Factor directly P_initial.
     // Since x0 is set to the provided mean, the b vector in the prior will be zero
-    // TODO(Frank): is there a reason why noiseModel is not simply P_initial?
+    // The reason why noiseModel is not simply P_initial is that a JacobianFactor only
+    // accepts Diagonal noise models.
     int n = traits<T>::GetDimension(x_initial);
     priorFactor_ = JacobianFactor::shared_ptr(
         new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),
@@ -82,20 +83,38 @@ namespace gtsam {
     const auto keys = motionFactor.keys();
 
     // Create a Gaussian Factor Graph
-    GaussianFactorGraph linearFactorGraph;
+    GaussianFactorGraph::shared_ptr linearFactorGraph = boost::make_shared<GaussianFactorGraph>();
 
     // Add in previous posterior as prior on the first state
-    linearFactorGraph.push_back(priorFactor_);
+    linearFactorGraph->push_back(priorFactor_);
 
     // Linearize motion model and add it to the Kalman Filter graph
     Values linearizationPoint;
-    linearizationPoint.insert(keys[0], x_);
-    linearizationPoint.insert(keys[1], x_); // TODO should this really be x_ ?
-    linearFactorGraph.push_back(motionFactor.linearize(linearizationPoint));
+
+    if (lastLinearization != nullptr) {
+      auto new_prior = boost::make_shared<JacobianFactor>(*priorFactor_);
+      new_prior->keys()[0] = lastLinearizationPointKey;
+
+      Values lastLinearizationPoint;
+      lastLinearizationPoint.insert(lastLinearizationPointKey, x_);
+      lastLinearizationPoint.insert(keys[0], x_);
+      T linearEstimate = solve_(*lastLinearization, lastLinearizationPoint, keys[0], &new_prior);
+
+      linearizationPoint.insert(keys[0], x_);
+      linearizationPoint.insert(keys[1], linearEstimate);
+    } else {
+      linearizationPoint.insert(keys[0], x_);
+      linearizationPoint.insert(keys[1], x_);
+    }
+
+    linearFactorGraph->push_back(motionFactor.linearize(linearizationPoint));
 
     // Solve the factor graph and update the current state estimate
     // and the posterior for the next iteration.
-    x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_);
+    x_ = solve_(*linearFactorGraph, linearizationPoint, keys[1], &priorFactor_);
+
+    lastLinearization.swap(linearFactorGraph);
+    lastLinearizationPointKey = keys[0];
 
     return x_;
   }

gtsam/nonlinear/ExtendedKalmanFilter.h

@@ -21,6 +21,7 @@
 
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
 
@@ -58,6 +59,8 @@ class ExtendedKalmanFilter {
  protected:
   T x_;                                     // linearization point
   JacobianFactor::shared_ptr priorFactor_;  // Gaussian density on x_
+  GaussianFactorGraph::shared_ptr lastLinearization;
+  Key lastLinearizationPointKey;
 
   static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints,
                   Key x, JacobianFactor::shared_ptr* newPrior);