#include <KalmanFilter.h>
Public Types | |
| enum | Factorization { QR, CHOLESKY } |
| typedef GaussianDensity::shared_ptr | State |
Public Member Functions | |
| State | init (const Vector &x0, const SharedDiagonal &P0) const |
| State | init (const Vector &x0, const Matrix &P0) const |
| version of init with a full covariance matrix More... | |
| KalmanFilter (size_t n, Factorization method=KALMANFILTER_DEFAULT_FACTORIZATION) | |
| State | predict (const State &p, const Matrix &F, const Matrix &B, const Vector &u, const SharedDiagonal &modelQ) const |
| State | predict2 (const State &p, const Matrix &A0, const Matrix &A1, const Vector &b, const SharedDiagonal &model) const |
| State | predictQ (const State &p, const Matrix &F, const Matrix &B, const Vector &u, const Matrix &Q) const |
| void | print (const std::string &s="") const |
| print More... | |
| 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 &Q) const |
Static Public Member Functions | |
| static Key | step (const State &p) |
Private Member Functions | |
| State | fuse (const State &p, GaussianFactor::shared_ptr newFactor) const |
| State | solve (const GaussianFactorGraph &factorGraph) const |
Private Attributes | |
| const GaussianFactorGraph::Eliminate | function_ |
| const Matrix | I_ |
| const size_t | n_ |
Kalman Filter class
Knows how to maintain a Gaussian density under linear-Gaussian motion and measurement models. It uses the square-root information form, as usual in GTSAM.
The filter is functional, in that it does not have state: you call init() to create an initial state, then predict() and update() that create new states out of an old state.
Definition at line 41 of file KalmanFilter.h.
The Kalman filter state is simply a GaussianDensity
Definition at line 56 of file KalmanFilter.h.
This Kalman filter is a Square-root Information filter The type below allows you to specify the factorization variant.
| Enumerator | |
|---|---|
| QR | |
| CHOLESKY | |
Definition at line 49 of file KalmanFilter.h.
|
inline |
Definition at line 70 of file KalmanFilter.h.
|
private |
Definition at line 56 of file KalmanFilter.cpp.
| KalmanFilter::State gtsam::KalmanFilter::init | ( | const Vector & | x0, |
| const SharedDiagonal & | P0 | ||
| ) | const |
Create initial state, i.e., prior density at time k=0 In Kalman Filter notation, these are x_{0|0} and P_{0|0}
| x0 | estimate at time 0 |
| P0 | covariance at time 0, given as a diagonal Gaussian 'model' |
Definition at line 67 of file KalmanFilter.cpp.
| KalmanFilter::State gtsam::KalmanFilter::init | ( | const Vector & | x0, |
| const Matrix & | P0 | ||
| ) | const |
version of init with a full covariance matrix
Definition at line 77 of file KalmanFilter.cpp.
| KalmanFilter::State gtsam::KalmanFilter::predict | ( | const State & | p, |
| const Matrix & | F, | ||
| const Matrix & | B, | ||
| const Vector & | u, | ||
| const SharedDiagonal & | modelQ | ||
| ) | const |
Predict the state P(x_{t+1}|Z^t) In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} Details and parameters: In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w where F is the state transition model/matrix, B is the control input model, and w is zero-mean, Gaussian white noise with covariance Q.
Definition at line 91 of file KalmanFilter.cpp.
| KalmanFilter::State gtsam::KalmanFilter::predict2 | ( | const State & | p, |
| const Matrix & | A0, | ||
| const Matrix & | A1, | ||
| const Vector & | b, | ||
| const SharedDiagonal & | model | ||
| ) | const |
Predict the state P(x_{t+1}|Z^t) In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} After the call, that is the density that can be queried. Details and parameters: This version of predict takes GaussianFactor motion model [A0 A1 b] with an optional noise model.
Definition at line 128 of file KalmanFilter.cpp.
| KalmanFilter::State gtsam::KalmanFilter::predictQ | ( | const State & | p, |
| const Matrix & | F, | ||
| const Matrix & | B, | ||
| const Vector & | u, | ||
| const Matrix & | Q | ||
| ) | const |
Definition at line 102 of file KalmanFilter.cpp.
| void gtsam::KalmanFilter::print | ( | const std::string & | s = "" | ) | const |
Definition at line 86 of file KalmanFilter.cpp.
|
private |
algorithm
Definition at line 40 of file KalmanFilter.cpp.
Return step index k, starts at 0, incremented at each predict.
Definition at line 92 of file KalmanFilter.h.
| KalmanFilter::State gtsam::KalmanFilter::update | ( | const State & | p, |
| const Matrix & | H, | ||
| const Vector & | z, | ||
| const SharedDiagonal & | model | ||
| ) | const |
Update Kalman filter with a measurement For the Kalman Filter, the measurement function, h(x_{t}) = z_{t} will be of the form h(x_{t}) = H*x_{t} + v where H is the observation model/matrix, and v is zero-mean, Gaussian white noise with covariance R. In this version, R is restricted to diagonal Gaussians (model parameter)
Definition at line 137 of file KalmanFilter.cpp.
| KalmanFilter::State gtsam::KalmanFilter::updateQ | ( | const State & | p, |
| const Matrix & | H, | ||
| const Vector & | z, | ||
| const Matrix & | Q | ||
| ) | const |
Definition at line 147 of file KalmanFilter.cpp.
|
private |
identity matrix of size n*n
Definition at line 62 of file KalmanFilter.h.
|
private |
dimensionality of state
Definition at line 61 of file KalmanFilter.h.
|
private |
Definition at line 60 of file KalmanFilter.h.