Public Types | Public Member Functions | Static Public Member Functions | Private Member Functions | Private Attributes | List of all members
gtsam::KalmanFilter Class Reference

#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_
 

Detailed Description

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.

Member Typedef Documentation

◆ State

The Kalman filter state is simply a GaussianDensity

Definition at line 56 of file KalmanFilter.h.

Member Enumeration Documentation

◆ Factorization

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.

Constructor & Destructor Documentation

◆ KalmanFilter()

gtsam::KalmanFilter::KalmanFilter ( size_t  n,
Factorization  method = KALMANFILTER_DEFAULT_FACTORIZATION 
)
inline

Definition at line 70 of file KalmanFilter.h.

Member Function Documentation

◆ fuse()

KalmanFilter::State gtsam::KalmanFilter::fuse ( const State p,
GaussianFactor::shared_ptr  newFactor 
) const
private

Definition at line 53 of file KalmanFilter.cpp.

◆ init() [1/2]

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}

Parameters
x0estimate at time 0
P0covariance at time 0, given as a diagonal Gaussian 'model'

Definition at line 65 of file KalmanFilter.cpp.

◆ init() [2/2]

KalmanFilter::State gtsam::KalmanFilter::init ( const Vector x0,
const Matrix P0 
) const

version of init with a full covariance matrix

Definition at line 75 of file KalmanFilter.cpp.

◆ predict()

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 89 of file KalmanFilter.cpp.

◆ predict2()

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 126 of file KalmanFilter.cpp.

◆ predictQ()

KalmanFilter::State gtsam::KalmanFilter::predictQ ( const State p,
const Matrix F,
const Matrix B,
const Vector u,
const Matrix Q 
) const

Definition at line 100 of file KalmanFilter.cpp.

◆ print()

void gtsam::KalmanFilter::print ( const std::string &  s = "") const

print

Definition at line 84 of file KalmanFilter.cpp.

◆ solve()

KalmanFilter::State gtsam::KalmanFilter::solve ( const GaussianFactorGraph factorGraph) const
private

algorithm

Definition at line 37 of file KalmanFilter.cpp.

◆ step()

static Key gtsam::KalmanFilter::step ( const State p)
inlinestatic

Return step index k, starts at 0, incremented at each predict.

Definition at line 92 of file KalmanFilter.h.

◆ update()

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 135 of file KalmanFilter.cpp.

◆ updateQ()

KalmanFilter::State gtsam::KalmanFilter::updateQ ( const State p,
const Matrix H,
const Vector z,
const Matrix Q 
) const

Definition at line 145 of file KalmanFilter.cpp.

Member Data Documentation

◆ function_

const GaussianFactorGraph::Eliminate gtsam::KalmanFilter::function_
private

identity matrix of size n*n

Definition at line 62 of file KalmanFilter.h.

◆ I_

const Matrix gtsam::KalmanFilter::I_
private

dimensionality of state

Definition at line 61 of file KalmanFilter.h.

◆ n_

const size_t gtsam::KalmanFilter::n_
private

Definition at line 60 of file KalmanFilter.h.


The documentation for this class was generated from the following files:


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:46:22