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

typedef GaussianDensity::shared_ptr gtsam::KalmanFilter::State

The Kalman filter state is simply a GaussianDensity

Definition at line 56 of file KalmanFilter.h.

Member Enumeration Documentation

Factorization

enum gtsam::KalmanFilter::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

x0	estimate at time 0
P0	covariance 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:

• KalmanFilter.h
• KalmanFilter.cpp