Merge pull request #1924 from borglab/feature/kf_docs

Kalman filter update

gtsam/linear/KalmanFilter.cpp

@@ -20,29 +20,44 @@
  * @author Frank Dellaert
  */
 
+
 #include <gtsam/linear/KalmanFilter.h>
+
+#include <gtsam/base/Testable.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/HessianFactor.h>
-#include <gtsam/base/Testable.h>
 
 #ifndef NDEBUG
 #include <cassert>
 #endif
 
-using namespace std;
+// In the code below we often get a covariance matrix Q, and we need to create a
+// JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as
+//   Q = L L^T
+// and hence
+//   Q^{-1} = L^{-T} L^{-1} = M^T M, with M = L^{-1}
+// We then have
+//   0.5 * |Ax - b|^T Q^{-1} |Ax - b|
+// = 0.5 * |Ax - b|^T M^T M |Ax - b|
+// = 0.5 * |MAx - Mb|^T |MAx - Mb|
+// The functor below efficiently multiplies with M by calling L.solve().
+namespace {
+using Matrix = gtsam::Matrix;
+struct InverseL : public Eigen::LLT<Matrix> {
+  InverseL(const Matrix& Q) : Eigen::LLT<Matrix>(Q) {}
+  Matrix operator*(const Matrix& A) const { return matrixL().solve(A); }
+};
+}  // namespace
 
 namespace gtsam {
-
 /* ************************************************************************* */
-// Auxiliary function to solve factor graph and return pointer to root conditional
+// Auxiliary function to solve factor graph and return pointer to root
-KalmanFilter::State //
+// conditional
-KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
+KalmanFilter::State KalmanFilter::solve(
-
+    const GaussianFactorGraph& factorGraph) const {
   // Eliminate the graph using the provided Eliminate function
   Ordering ordering(factorGraph.keys());
-  const auto bayesNet = //
+  const auto bayesNet = factorGraph.eliminateSequential(ordering, function_);
-      factorGraph.eliminateSequential(ordering, function_);
 
   // As this is a filter, all we need is the posterior P(x_t).
   // This is the last GaussianConditional in the resulting BayesNet
@@ -52,9 +67,8 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
 
 /* ************************************************************************* */
 // Auxiliary function to create a small graph for predict or update and solve
-KalmanFilter::State //
+KalmanFilter::State KalmanFilter::fuse(
-KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
+    const State& p, GaussianFactor::shared_ptr newFactor) const {
-
   // Create a factor graph
   GaussianFactorGraph factorGraph;
   factorGraph.push_back(p);
@@ -67,33 +81,41 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::init(const Vector& x0,
                                        const SharedDiagonal& P0) const {
-
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
-  factorGraph.emplace_shared<JacobianFactor>(0, I_, x0, P0); // |x-x0|^2_diagSigma
+  factorGraph.emplace_shared<JacobianFactor>(0, I_, x0,
+                                             P0);  // |x-x0|^2_P0
   return solve(factorGraph);
 }
 
 /* ************************************************************************* */
-KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const {
+KalmanFilter::State KalmanFilter::init(const Vector& x0,
-
+                                       const Matrix& P0) const {
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
-  factorGraph.emplace_shared<HessianFactor>(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
+
+  // Perform Cholesky decomposition of P0 = LL^T
+  InverseL M(P0);
+
+  // Premultiply I and x0 with M=L^{-1}
+  const Matrix A = M * I_;  // = M
+  const Vector b = M * x0;  // = M*x0
+  factorGraph.emplace_shared<JacobianFactor>(0, A, b);
   return solve(factorGraph);
 }
 
 /* ************************************************************************* */
-void KalmanFilter::print(const string& s) const {
+void KalmanFilter::print(const std::string& s) const {
-  cout << "KalmanFilter " << s << ", dim = " << n_ << endl;
+  std::cout << "KalmanFilter " << s << ", dim = " << n_ << std::endl;
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
-    const Matrix& B, const Vector& u, const SharedDiagonal& model) const {
+                                          const Matrix& B, const Vector& u,
-
+                                          const SharedDiagonal& model) const {
   // The factor related to the motion model is defined as
-  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
+  // f2(x_{k},x_{k+1}) =
+  // (F*x_{k} + B*u - x_{k+1}) * Q^-1 * (F*x_{k} + B*u - x_{k+1})^T
   Key k = step(p);
   return fuse(p,
               std::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
@@ -101,10 +123,11 @@ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
-    const Matrix& B, const Vector& u, const Matrix& Q) const {
+                                           const Matrix& B, const Vector& u,
-
+                                           const Matrix& Q) const {
 #ifndef NDEBUG
-  DenseIndex n = F.cols();
+  const DenseIndex n = dim();
+  assert(F.cols() == n);
   assert(F.rows() == n);
   assert(B.rows() == n);
   assert(B.cols() == u.size());
@@ -112,50 +135,52 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
   assert(Q.cols() == n);
 #endif
 
-  // The factor related to the motion model is defined as
+  // Perform Cholesky decomposition of Q = LL^T
-  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
+  InverseL M(Q);
-  // See documentation in HessianFactor, we have A1 = -F,  A2 = I_, b = B*u:
+
-  // TODO: starts to seem more elaborate than straight-up KF equations?
+  // Premultiply -F, I, and B * u with M=L^{-1}
-  Matrix M = Q.inverse(), Ft = trans(F);
+  const Matrix A1 = -(M * F);
-  Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
+  const Matrix A2 = M * I_;
-  Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
+  const Vector b = M * (B * u);
-  double f = dot(b, g2);
+  return predict2(p, A1, A2, b);
-  Key k = step(p);
-  return fuse(p,
-      std::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f));
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
-    const Matrix& A1, const Vector& b, const SharedDiagonal& model) const {
+                                           const Matrix& A1, const Vector& b,
+                                           const SharedDiagonal& model) const {
   // Nhe factor related to the motion model is defined as
-  // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
+  // f2(x_{k},x_{k+1}) = |A0*x_{k} + A1*x_{k+1} - b|^2
   Key k = step(p);
   return fuse(p, std::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model));
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
-    const Vector& z, const SharedDiagonal& model) const {
+                                         const Vector& z,
+                                         const SharedDiagonal& model) const {
   // The factor related to the measurements would be defined as
-  // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
+  // f2 = (h(x_{k}) - z_{k}) * R^-1 * (h(x_{k}) - z_{k})^T
-  //    = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
+  //    = (x_{k} - z_{k}) * R^-1 * (x_{k} - z_{k})^T
   Key k = step(p);
   return fuse(p, std::make_shared<JacobianFactor>(k, H, z, model));
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
-    const Vector& z, const Matrix& Q) const {
+                                          const Vector& z,
+                                          const Matrix& Q) const {
   Key k = step(p);
-  Matrix M = Q.inverse(), Ht = trans(H);
+
-  Matrix G = Ht * M * H;
+  // Perform Cholesky decomposition of Q = LL^T
-  Vector g = Ht * M * z;
+  InverseL M(Q);
-  double f = dot(z, M * z);
+
-  return fuse(p, std::make_shared<HessianFactor>(k, G, g, f));
+  // Pre-multiply H and z with M=L^{-1}, respectively
+  const Matrix A = M * H;
+  const Vector b = M * z;
+  return fuse(p, std::make_shared<JacobianFactor>(k, A, b));
 }
 
 /* ************************************************************************* */
 
-} // \namespace gtsam
+}  // namespace gtsam
-

gtsam/linear/KalmanFilter.h

@@ -11,10 +11,10 @@
 
 /**
  * @file KalmanFilter.h
- * @brief Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
+ * @brief Simple linear Kalman filter implemented using factor graphs, i.e.,
+ * performs Cholesky or QR-based SRIF (Square-Root Information Filter).
  * @date Sep 3, 2011
- * @author Stephen Williams
+ * @authors Stephen Williams, Frank Dellaert
- * @author Frank Dellaert
  */
 
 #pragma once
@@ -32,120 +32,186 @@ namespace gtsam {
 /**
  * Kalman Filter class
  *
- * Knows how to maintain a Gaussian density under linear-Gaussian motion and
+ * Maintains a Gaussian density under linear-Gaussian motion and
- * measurement models. It uses the square-root information form, as usual in GTSAM.
+ * measurement models using the square-root information form.
  *
- * The filter is functional, in that it does not have state: you call init() to create
+ * The filter is functional; it does not maintain internal state. Instead:
- * an initial state, then predict() and update() that create new states out of an old state.
+ * - Use `init()` to create an initial filter state,
+ * - Call `predict()` and `update()` to create new states.
  */
 class GTSAM_EXPORT KalmanFilter {
-
  public:
-
   /**
-   *  This Kalman filter is a Square-root Information filter
+   * @enum Factorization
-   *  The type below allows you to specify the factorization variant.
+   * @brief Specifies the factorization variant to use.
    */
-  enum Factorization {
+  enum Factorization { QR, CHOLESKY };
-    QR, CHOLESKY
-  };
 
   /**
-   * The Kalman filter state is simply a GaussianDensity
+   * @typedef State
+   * @brief The Kalman filter state, represented as a shared pointer to a
+   * GaussianDensity.
    */
   typedef GaussianDensity::shared_ptr State;
 
  private:
+  const size_t n_;  ///< Dimensionality of the state.
+  const Matrix I_;  ///< Identity matrix of size \f$ n \times n \f$.
+  const GaussianFactorGraph::Eliminate
+      function_;  ///< Elimination algorithm used.
 
-  const size_t n_; /** dimensionality of state */
+  /**
-  const Matrix I_; /** identity matrix of size n*n */
+   * Solve the factor graph.
-  const GaussianFactorGraph::Eliminate function_; /** algorithm */
+   * @param factorGraph The Gaussian factor graph to solve.
-
+   * @return The resulting Kalman filter state.
+   */
   State solve(const GaussianFactorGraph& factorGraph) const;
+
+  /**
+   * Fuse two states.
+   * @param p The prior state.
+   * @param newFactor The new factor to incorporate.
+   * @return The resulting fused state.
+   */
   State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
 
  public:
-
+  /**
-  // Constructor
+   * Constructor.
-  KalmanFilter(size_t n, Factorization method =
+   * @param n Dimensionality of the state.
-      KALMANFILTER_DEFAULT_FACTORIZATION) :
+   * @param method Factorization method (default: QR unless compile-flag set).
-      n_(n), I_(Matrix::Identity(n_, n_)), function_(
+   */
-          method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
+  KalmanFilter(size_t n,
-              GaussianFactorGraph::Eliminate(EliminateCholesky)) {
+               Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION)
-  }
+      : n_(n),
+        I_(Matrix::Identity(n_, n_)),
+        function_(method == QR
+                      ? GaussianFactorGraph::Eliminate(EliminateQR)
+                      : GaussianFactorGraph::Eliminate(EliminateCholesky)) {}
 
   /**
-   * Create initial state, i.e., prior density at time k=0
+   * Create the initial state (prior density at time \f$ k=0 \f$).
-   * In Kalman Filter notation, these are x_{0|0} and P_{0|0}
+   *
-   * @param x0 estimate at time 0
+   * In Kalman Filter notation:
-   * @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
+   * - \f$ x_{0|0} \f$: Initial state estimate.
+   * - \f$ P_{0|0} \f$: Initial covariance matrix.
+   *
+   * @param x0 Estimate of the state at time 0 (\f$ x_{0|0} \f$).
+   * @param P0 Covariance matrix (\f$ P_{0|0} \f$), given as a diagonal Gaussian
+   * model.
+   * @return Initial Kalman filter state.
    */
   State init(const Vector& x0, const SharedDiagonal& P0) const;
 
-  /// version of init with a full covariance matrix
+  /**
+   * Create the initial state with a full covariance matrix.
+   * @param x0 Initial state estimate.
+   * @param P0 Full covariance matrix.
+   * @return Initial Kalman filter state.
+   */
   State init(const Vector& x0, const Matrix& P0) const;
 
-  /// print
+  /**
+   * Print the Kalman filter details.
+   * @param s Optional string prefix.
+   */
   void print(const std::string& s = "") const;
 
-  /** Return step index k, starts at 0, incremented at each predict. */
+  /**
-  static Key step(const State& p) {
+   * Return the step index \f$ k \f$ (starts at 0, incremented at each predict
-    return p->firstFrontalKey();
+   * step).
-  }
+   * @param p The current state.
+   * @return Step index.
+   */
+  static Key step(const State& p) { return p->firstFrontalKey(); }
 
   /**
-   * Predict the state P(x_{t+1}|Z^t)
+   * Predict the next state \f$ P(x_{k+1}|Z^k) \f$.
-   *   In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
+   *
-   * Details and parameters:
+   * In Kalman Filter notation:
-   *   In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w
+   * - \f$ x_{k+1|k} \f$: Predicted state.
-   *   where F is the state transition model/matrix, B is the control input model,
+   * - \f$ P_{k+1|k} \f$: Predicted covariance.
-   *   and w is zero-mean, Gaussian white noise with covariance Q.
+   *
+   * Motion model:
+   * \f[
+   * x_{k+1} = F \cdot x_k + B \cdot u_k + w
+   * \f]
+   * where \f$ w \f$ is zero-mean Gaussian noise with covariance \f$ Q \f$.
+   *
+   * @param p Previous state (\f$ x_k \f$).
+   * @param F State transition matrix (\f$ F \f$).
+   * @param B Control input matrix (\f$ B \f$).
+   * @param u Control vector (\f$ u_k \f$).
+   * @param modelQ Noise model (\f$ Q \f$, diagonal Gaussian).
+   * @return Predicted state (\f$ x_{k+1|k} \f$).
    */
   State predict(const State& p, const Matrix& F, const Matrix& B,
                 const Vector& u, const SharedDiagonal& modelQ) const;
 
-  /*
+  /**
-   *  Version of predict with full covariance
+   * Predict the next state with a full covariance matrix.
-   *  Q is normally derived as G*w*G^T where w models uncertainty of some
+   *
-   *  physical property, such as velocity or acceleration, and G is derived from physics.
+   *@note Q is normally derived as G*w*G^T where w models uncertainty of some
-   *  This version allows more realistic models than a diagonal covariance matrix.
+   * physical property, such as velocity or acceleration, and G is derived from
+   * physics. This version allows more realistic models than a diagonal matrix.
+   *
+   * @param p Previous state.
+   * @param F State transition matrix.
+   * @param B Control input matrix.
+   * @param u Control vector.
+   * @param Q Full covariance matrix (\f$ Q \f$).
+   * @return Predicted state.
    */
   State predictQ(const State& p, const Matrix& F, const Matrix& B,
                  const Vector& u, const Matrix& Q) const;
 
   /**
-   * Predict the state P(x_{t+1}|Z^t)
+   * Predict the next state using a GaussianFactor motion model.
-   *   In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
+   * @param p Previous state.
-   *   After the call, that is the density that can be queried.
+   * @param A0 Factor matrix.
-   * Details and parameters:
+   * @param A1 Factor matrix.
-   *   This version of predict takes GaussianFactor motion model [A0 A1 b]
+   * @param b Constant term vector.
-   *   with an optional noise model.
+   * @param model Noise model (optional).
+   * @return Predicted state.
    */
   State predict2(const State& p, const Matrix& A0, const Matrix& A1,
-      const Vector& b, const SharedDiagonal& model) const;
+                 const Vector& b, const SharedDiagonal& model = nullptr) const;
 
   /**
-   * Update Kalman filter with a measurement
+   * Update the Kalman filter with a measurement.
-   * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t}
+   *
-   * will be of the form h(x_{t}) = H*x_{t} + v
+   * Observation model:
-   * where H is the observation model/matrix, and v is zero-mean,
+   * \f[
-   * Gaussian white noise with covariance R.
+   * z_k = H \cdot x_k + v
+   * \f]
+   * where \f$ v \f$ is zero-mean Gaussian noise with covariance R.
    * In this version, R is restricted to diagonal Gaussians (model parameter)
+   *
+   * @param p Previous state.
+   * @param H Observation matrix.
+   * @param z Measurement vector.
+   * @param model Noise model (diagonal Gaussian).
+   * @return Updated state.
    */
   State update(const State& p, const Matrix& H, const Vector& z,
                const SharedDiagonal& model) const;
 
-  /*
+  /**
-   *  Version of update with full covariance
+   * Update the Kalman filter with a measurement using a full covariance matrix.
-   *  Q is normally derived as G*w*G^T where w models uncertainty of some
+   * @param p Previous state.
-   *  physical property, such as velocity or acceleration, and G is derived from physics.
+   * @param H Observation matrix.
-   *  This version allows more realistic models than a diagonal covariance matrix.
+   * @param z Measurement vector.
+   * @param R Full covariance matrix.
+   * @return Updated state.
    */
   State updateQ(const State& p, const Matrix& H, const Vector& z,
-      const Matrix& Q) const;
+                const Matrix& R) const;
+
+  /**
+   * Return the dimensionality of the state.
+   * @return Dimensionality of the state.
+   */
+  size_t dim() const { return n_; }
 };
 
-} // \namespace gtsam
+}  // namespace gtsam
-
-/* ************************************************************************* */
-