KalmanFilter.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
 
#ifndef KALMANFILTER_DEFAULT_FACTORIZATION
#define KALMANFILTER_DEFAULT_FACTORIZATION QR
#endif
 
namespace gtsam {
 
class GTSAM_EXPORT KalmanFilter {
 
public:
 
  enum Factorization {
    QR, CHOLESKY
  };
 
  typedef GaussianDensity::shared_ptr State;
 
private:
 
  const size_t n_; 
  const Matrix I_; 
  const GaussianFactorGraph::Eliminate function_; 
  State solve(const GaussianFactorGraph& factorGraph) const;
  State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
 
public:
 
  // Constructor
  KalmanFilter(size_t n, Factorization method =
      KALMANFILTER_DEFAULT_FACTORIZATION) :
      n_(n), I_(Matrix::Identity(n_, n_)), function_(
          method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
              GaussianFactorGraph::Eliminate(EliminateCholesky)) {
  }
 
  State init(const Vector& x0, const SharedDiagonal& P0) const;
 
  State init(const Vector& x0, const Matrix& P0) const;
 
  void print(const std::string& s = "") const;
 
  static Key step(const State& p) {
    return p->firstFrontalKey();
  }
 
  State predict(const State& p, const Matrix& F, const Matrix& B,
      const Vector& u, const SharedDiagonal& modelQ) const;
 
  /*
   *  Version of predict with full covariance
   *  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.
   *  This version allows more realistic models than a diagonal covariance matrix.
   */
  State predictQ(const State& p, const Matrix& F, const Matrix& B,
      const Vector& u, const Matrix& Q) const;
 
  State predict2(const State& p, const Matrix& A0, const Matrix& A1,
      const Vector& b, const SharedDiagonal& model) const;
 
  State update(const State& p, const Matrix& H, const Vector& z,
      const SharedDiagonal& model) const;
 
  /*
   *  Version of update with full covariance
   *  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.
   *  This version allows more realistic models than a diagonal covariance matrix.
   */
  State updateQ(const State& p, const Matrix& H, const Vector& z,
      const Matrix& Q) const;
};
 
} // \namespace gtsam
 
/* ************************************************************************* */