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.

Public Member Functions
	KalmanFilter (size_t n, Factorization method=KALMANFILTER_DEFAULT_FACTORIZATION)
	Constructor.
State	init (const Vector &x0, const SharedDiagonal &P0) const
	Create the initial state (prior density at time \( k=0 \)).
State	init (const Vector &x0, const Matrix &P0) const
	Create the initial state with a full covariance matrix.
void	print (const std::string &s="") const
	Print the Kalman filter details.
State	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) \).
State	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.
State	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.
State	update (const State &p, const Matrix &H, const Vector &z, const SharedDiagonal &model) const
	Update the Kalman filter with a measurement.
State	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.
size_t	dim () const
	Return the dimensionality of the state.

Static Public Member Functions
static Key	step (const State &p)
	Return the step index \( k \) (starts at 0, incremented at each predict step).

Public Types
enum	Factorization { QR , CHOLESKY }
	Specifies the factorization variant to use.
typedef GaussianDensity::shared_ptr	State
	The Kalman filter state, represented as a shared pointer to a GaussianDensity.

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).

Member Function Documentation

◆ dim()

size_t gtsam::KalmanFilter::dim ( ) const

inline

Return the dimensionality of the state.

Returns: Dimensionality of the state.

◆ 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.

◆ 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.

◆ 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 ( \( x_k \)).
F	State transition matrix ( \( F \)).
B	Control input matrix ( \( B \)).
u	Control vector ( \( u_k \)).
modelQ	Noise model ( \( Q \), diagonal Gaussian).

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

◆ 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.

◆ 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 ( \( Q \)).

Returns: Predicted state.

◆ print()

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

Print the Kalman filter details.

Parameters

s	Optional string prefix.

◆ step()

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.

◆ 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.

◆ 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.

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/linear/KalmanFilter.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/linear/KalmanFilter.cpp

Detailed Description

Public Member Functions

Static Public Member Functions

Public Types

Constructor & Destructor Documentation

◆ KalmanFilter()

Member Function Documentation

◆ dim()

◆ init() [1/2]

◆ init() [2/2]

◆ predict()

◆ predict2()

◆ predictQ()

◆ print()

◆ step()

◆ update()

◆ updateQ()