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:
  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;

  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 = nullptr) const;

  State update(const State& p, const Matrix& H, const Vector& z,
               const SharedDiagonal& model) const;

  State updateQ(const State& p, const Matrix& H, const Vector& z,
                const Matrix& R) const;

  size_t dim() const { return n_; }
};
 
}  // namespace gtsam