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

/* ************************************************************************* */