gtsam  4.1.0
gtsam
KalmanFilter.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #pragma once
21 
25 
26 #ifndef KALMANFILTER_DEFAULT_FACTORIZATION
27 #define KALMANFILTER_DEFAULT_FACTORIZATION QR
28 #endif
29 
30 namespace gtsam {
31 
41 class GTSAM_EXPORT KalmanFilter {
42 
43 public:
44 
50  QR, CHOLESKY
51  };
52 
56  typedef GaussianDensity::shared_ptr State;
57 
58 private:
59 
60  const size_t n_;
61  const Matrix I_;
62  const GaussianFactorGraph::Eliminate function_;
64  State solve(const GaussianFactorGraph& factorGraph) const;
65  State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
66 
67 public:
68 
69  // Constructor
70  KalmanFilter(size_t n, Factorization method =
71  KALMANFILTER_DEFAULT_FACTORIZATION) :
72  n_(n), I_(Matrix::Identity(n_, n_)), function_(
73  method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
74  GaussianFactorGraph::Eliminate(EliminateCholesky)) {
75  }
76 
83  State init(const Vector& x0, const SharedDiagonal& P0) const;
84 
86  State init(const Vector& x0, const Matrix& P0) const;
87 
89  void print(const std::string& s = "") const;
90 
92  static Key step(const State& p) {
93  return p->firstFrontalKey();
94  }
95 
104  State predict(const State& p, const Matrix& F, const Matrix& B,
105  const Vector& u, const SharedDiagonal& modelQ) const;
106 
107  /*
108  * Version of predict with full covariance
109  * Q is normally derived as G*w*G^T where w models uncertainty of some
110  * physical property, such as velocity or acceleration, and G is derived from physics.
111  * This version allows more realistic models than a diagonal covariance matrix.
112  */
113  State predictQ(const State& p, const Matrix& F, const Matrix& B,
114  const Vector& u, const Matrix& Q) const;
115 
124  State predict2(const State& p, const Matrix& A0, const Matrix& A1,
125  const Vector& b, const SharedDiagonal& model) const;
126 
135  State update(const State& p, const Matrix& H, const Vector& z,
136  const SharedDiagonal& model) const;
137 
138  /*
139  * Version of update with full covariance
140  * Q is normally derived as G*w*G^T where w models uncertainty of some
141  * physical property, such as velocity or acceleration, and G is derived from physics.
142  * This version allows more realistic models than a diagonal covariance matrix.
143  */
144  State updateQ(const State& p, const Matrix& H, const Vector& z,
145  const Matrix& Q) const;
146 };
147 
148 } // \namespace gtsam
149 
150 /* ************************************************************************* */
151 
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::KalmanFilter::Factorization
Factorization
This Kalman filter is a Square-root Information filter The type below allows you to specify the facto...
Definition: KalmanFilter.h:49
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::KalmanFilter::State
GaussianDensity::shared_ptr State
The Kalman filter state is simply a GaussianDensity.
Definition: KalmanFilter.h:56
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition: JacobianFactor.cpp:797
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
boost::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition: EliminateableFactorGraph.h:89
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
GaussianDensity.h
A Gaussian Density.
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::KalmanFilter::step
static Key step(const State &p)
Return step index k, starts at 0, incremented at each predict.
Definition: KalmanFilter.h:92
gtsam::KalmanFilter
Kalman Filter class.
Definition: KalmanFilter.h:41
NoiseModel.h