gtsam
4.0.0
gtsam

Kalman Filter class.
Knows how to maintain a Gaussian density under linearGaussian motion and measurement models. It uses the squareroot 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.
Public Member Functions  
KalmanFilter (size_t n, Factorization method=KALMANFILTER_DEFAULT_FACTORIZATION)  
State  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_{00} and P_{00}. More...  
State  init (const Vector &x0, const Matrix &P0) const 
version of init with a full covariance matrix  
void  print (const std::string &s="") const 
print  
State  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+1t} and P_{t+1t} 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 zeromean, Gaussian white noise with covariance Q.  
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) const 
Predict the state P(x_{t+1}Z^t) In Kalman Filter notation, this is x_{t+1t} and P_{t+1t} After the call, that is the density that can be queried. More...  
State  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 zeromean, Gaussian white noise with covariance R. More...  
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) 
Return step index k, starts at 0, incremented at each predict. More...  
Public Types  
enum  Factorization { QR, CHOLESKY } 
This Kalman filter is a Squareroot Information filter The type below allows you to specify the factorization variant.  
typedef GaussianDensity::shared_ptr  State 
The Kalman filter state is simply a GaussianDensity.  
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_{00} and P_{00}.
x0  estimate at time 0 
P0  covariance at time 0, given as a diagonal Gaussian 'model' 
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+1t} and P_{t+1t} 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.
Return step index k, starts at 0, incremented at each predict.
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 zeromean, Gaussian white noise with covariance R.
In this version, R is restricted to diagonal Gaussians (model parameter)