gtsam 4.1.1 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
30namespace gtsam {
31
41class GTSAM_EXPORT KalmanFilter {
42
43public:
44
50 QR, CHOLESKY
51 };
52
56 typedef GaussianDensity::shared_ptr State;
57
58private:
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
67public:
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
Linear Factor Graph where all factors are Gaussians.
A Gaussian Density.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
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:789
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
std::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
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:69
Kalman Filter class.
Definition: KalmanFilter.h:41
Factorization
This Kalman filter is a Square-root Information filter The type below allows you to specify the facto...
Definition: KalmanFilter.h:49
static Key step(const State &p)
Return step index k, starts at 0, incremented at each predict.
Definition: KalmanFilter.h:92
GaussianDensity::shared_ptr State
The Kalman filter state is simply a GaussianDensity.
Definition: KalmanFilter.h:56