gtsam  4.0.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
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
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
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
Kalman Filter class.
Definition: KalmanFilter.h:41
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:65
static Key step(const State &p)
Return step index k, starts at 0, incremented at each predict.
Definition: KalmanFilter.h:92
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:721
GaussianDensity::shared_ptr State
The Kalman filter state is simply a GaussianDensity.
Definition: KalmanFilter.h:56
Factorization
This Kalman filter is a Square-root Information filter The type below allows you to specify the facto...
Definition: KalmanFilter.h:49
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
A Gaussian Density.
Linear Factor Graph where all factors are Gaussians.
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
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42