#include <KalmanFilter.h>

Public Types
enum	Factorization { QR, CHOLESKY }
	Specifies the factorization variant to use. More...

typedef GaussianDensity::shared_ptr	State
	The Kalman filter state, represented as a shared pointer to a GaussianDensity. More...

Public Member Functions
size_t	dim () const

State	init (const Vector &x0, const Matrix &P0) const

State	init (const Vector &x0, const SharedDiagonal &P0) const

	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=nullptr) 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

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 &R) 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_
	Elimination algorithm used. More...

const Matrix	I_
	Identity matrix of size $n \times n$ . More...

const size_t	n_
	Dimensionality of the state. More...

Detailed Description

Kalman Filter class

Maintains a Gaussian density under linear-Gaussian motion and measurement models using the square-root information form.

The filter is functional; it does not maintain internal state. Instead:

Use init() to create an initial filter state,
Call predict() and update() to create new states.

Definition at line 42 of file KalmanFilter.h.

Member Typedef Documentation

◆ State

gtsam::KalmanFilter::State

The Kalman filter state, represented as a shared pointer to a GaussianDensity.

Definition at line 55 of file KalmanFilter.h.

Member Enumeration Documentation

◆ Factorization

enum gtsam::KalmanFilter::Factorization

Specifies the factorization variant to use.

Enumerator
QR
CHOLESKY

Definition at line 48 of file KalmanFilter.h.

Constructor & Destructor Documentation

◆ KalmanFilter()

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

inline

Constructor.

Parameters

n	Dimensionality of the state.
method	Factorization method (default: QR unless compile-flag set).

Definition at line 84 of file KalmanFilter.h.

Member Function Documentation

◆ dim()

size_t gtsam::KalmanFilter::dim ( ) const

inline

Return the dimensionality of the state.

Returns: Dimensionality of the state.

Definition at line 214 of file KalmanFilter.h.

◆ fuse()

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

private

Fuse two states.

Parameters

p	The prior state.
newFactor	The new factor to incorporate.

Returns: The resulting fused state.

Definition at line 70 of file KalmanFilter.cpp.

◆ init() [1/2]

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

Create the initial state with a full covariance matrix.

Parameters

x0	Initial state estimate.
P0	Full covariance matrix.

Returns: Initial Kalman filter state.

Definition at line 92 of file KalmanFilter.cpp.

◆ init() [2/2]

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

Create the initial state (prior density at time $ k=0 $ ).

In Kalman Filter notation:

$x_{0|0}$ : Initial state estimate.
$P_{0|0}$ : Initial covariance matrix.

Parameters

x0	Estimate of the state at time 0 ( $x_{0\|0}$ ).
P0	Covariance matrix ( $P_{0\|0}$ ), given as a diagonal Gaussian model.

Returns: Initial Kalman filter state.

Definition at line 82 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 next state $P(x_{k+1}|Z^k)$ .

In Kalman Filter notation:

$x_{k+1|k}$ : Predicted state.
$P_{k+1|k}$ : Predicted covariance.

Motion model:

$x_{k+1} = F \cdot x_k + B \cdot u_k + w$

where $ w $ is zero-mean Gaussian noise with covariance $ Q $ .

Parameters

p	Previous state ( ).
F	State transition matrix ( ).
B	Control input matrix ( ).
u	Control vector ( ).
modelQ	Noise model ( , diagonal Gaussian).

Returns: Predicted state ( $x_{k+1|k}$ ).

Definition at line 113 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 = `nullptr`
	)		const

Predict the next state using a GaussianFactor motion model.

Parameters

p	Previous state.
A0	Factor matrix.
A1	Factor matrix.
b	Constant term vector.
model	Noise model (optional).

Returns: Predicted state.

Definition at line 149 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

Predict the next state with a full covariance matrix.

Note: Q is normally derived as G*w*G^T where w models uncertainty of some physical property, such as velocity or acceleration, and G is derived from physics. This version allows more realistic models than a diagonal matrix.

Parameters

p	Previous state.
F	State transition matrix.
B	Control input matrix.
u	Control vector.
Q	Full covariance matrix ( ).

Returns: Predicted state.

Definition at line 125 of file KalmanFilter.cpp.

◆ print()

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

Print the Kalman filter details.

Parameters

s	Optional string prefix.

Definition at line 108 of file KalmanFilter.cpp.

◆ solve()

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

private

Solve the factor graph.

Parameters

factorGraph The Gaussian factor graph to solve.

Returns: The resulting Kalman filter state.

Definition at line 56 of file KalmanFilter.cpp.

◆ step()

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

inlinestatic

Return the step index $ k $ (starts at 0, incremented at each predict step).

Parameters

p	The current state.

Returns: Step index.

Definition at line 126 of file KalmanFilter.h.

◆ update()

KalmanFilter::State gtsam::KalmanFilter::update	(	const State &	p,
		const Matrix &	H,
		const Vector &	z,
		const SharedDiagonal &	model
	)		const

Update the Kalman filter with a measurement.

Observation model:

$z_k = H \cdot x_k + v$

where $ v $ is zero-mean Gaussian noise with covariance R. In this version, R is restricted to diagonal Gaussians (model parameter)

Parameters

p	Previous state.
H	Observation matrix.
z	Measurement vector.
model	Noise model (diagonal Gaussian).

Returns: Updated state.

Definition at line 159 of file KalmanFilter.cpp.

◆ updateQ()

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

Update the Kalman filter with a measurement using a full covariance matrix.

Parameters

p	Previous state.
H	Observation matrix.
z	Measurement vector.
R	Full covariance matrix.

Returns: Updated state.

Definition at line 170 of file KalmanFilter.cpp.

Member Data Documentation

◆ function_

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

private

Elimination algorithm used.

Definition at line 61 of file KalmanFilter.h.

◆ I_

const Matrix gtsam::KalmanFilter::I_

private

Identity matrix of size $n \times n$ .

Definition at line 59 of file KalmanFilter.h.

◆ n_

const size_t gtsam::KalmanFilter::n_

private

Dimensionality of the state.

Definition at line 58 of file KalmanFilter.h.

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

Public Types

Public Member Functions

Static Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ State

Member Enumeration Documentation

◆ Factorization

Constructor & Destructor Documentation

◆ KalmanFilter()

Member Function Documentation

◆ dim()

◆ fuse()

◆ init() [1/2]

◆ init() [2/2]

◆ predict()

◆ predict2()

◆ predictQ()

◆ print()

◆ solve()

◆ step()

◆ update()

◆ updateQ()

Member Data Documentation

◆ function_

◆ I_

◆ n_