Use LLT rather than inverse

gtsam/linear/KalmanFilter.cpp

@@ -20,26 +20,35 @@
  * @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/linear/KalmanFilter.h>
-#include <gtsam/base/Testable.h>
-
 
 using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
+// In the code below we often need to multiply matrices with R, the Cholesky
-// Auxiliary function to solve factor graph and return pointer to root conditional
+// factor of the information matrix Q^{-1}, when given a covariance matrix Q.
-KalmanFilter::State //
+// We can do this efficiently using Eigen, as we have:
-KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
+//   Q = L L^T
+//   => Q^{-1} = L^{-T}L^{-1} = R^T R
+//   => L^{-1} = R
+// Rather than explicitly calculate R and do R*A, we should use L.solve(A)
+// The following macro makes L available in the scope where it is used:
+#define FACTORIZE_Q_INTO_L(Q)       \
+  Eigen::LLT<Matrix> llt(Q); \
+  auto L = llt.matrixL();
 
+/* ************************************************************************* */
+// Auxiliary function to solve factor graph and return pointer to root
+// conditional
+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
@@ -49,9 +58,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);
@@ -64,19 +72,26 @@ 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
+  FACTORIZE_Q_INTO_L(P0)
+
+  // Premultiply I and x0 with R
+  const Matrix R = L.solve(I_);  // = R
+  const Vector b = L.solve(x0);  // = R*x0
+  factorGraph.emplace_shared<JacobianFactor>(0, R, b);
   return solve(factorGraph);
 }
 
@@ -87,10 +102,11 @@ void KalmanFilter::print(const string& s) const {
 
 /* ************************************************************************* */
 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));
@@ -98,10 +114,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());
@@ -109,50 +126,53 @@ 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
-  // 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
+  FACTORIZE_Q_INTO_L(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 A1 = -F and A2 = I, b = B * u with R
-  Matrix M = Q.inverse(), Ft = trans(F);
+  const Matrix A1 = -L.solve(F);    // = -R*F
-  Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
+  const Matrix A2 = L.solve(I_);    // = R
-  Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
+  const Vector b = L.solve(B * u);  // = R * (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
-  Vector g = Ht * M * z;
+  FACTORIZE_Q_INTO_L(Q)
-  double f = dot(z, M * z);
+
-  return fuse(p, std::make_shared<HessianFactor>(k, G, g, f));
+  // Pre-multiply H and z with R, respectively
+  const Matrix RtH = L.solve(H);  // = R * H
+  const Vector b = L.solve(z);    // = R * z
+
+  return fuse(p, std::make_shared<JacobianFactor>(k, RtH, b));
 }
 
 /* ************************************************************************* */
 
-} // \namespace gtsam
+}  // namespace gtsam
-