#include <visp3/core/vpKalmanFilter.h>

Inheritance diagram for vpKalmanFilter:

Public Member Functions
	vpKalmanFilter ()

	vpKalmanFilter (unsigned int n_signal)

	vpKalmanFilter (unsigned int size_state, unsigned int size_measure, unsigned int n_signal)

virtual	~vpKalmanFilter ()

void	setNumberOfSignal (unsigned int n_signal)

void	init (unsigned int size_state, unsigned int size_measure, unsigned int n_signal)

void	prediction ()

void	filtering (const vpColVector &z)

unsigned int	getStateSize ()

unsigned int	getMeasureSize ()

unsigned int	getNumberOfSignal ()

long	getIteration ()

void	verbose (bool on)

Public Attributes
vpColVector	Xest

vpColVector	Xpre

vpMatrix	F

vpMatrix	H

vpMatrix	R

vpMatrix	Q

double	dt

vpMatrix	Ppre

vpMatrix	Pest

Protected Attributes
long	iter

unsigned int	size_state

unsigned int	size_measure

unsigned int	nsignal

bool	verbose_mode

vpMatrix	W

vpMatrix	I

Detailed Description

This class provides a generic Kalman filtering algorithm along with some specific state model (constant velocity, constant acceleration) which are implemented in the vpLinearKalmanFilterInstantiation class.

The state evolution equation is given by:

${\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\$

where ${\bf x}_{k}$ is the unknown state at iteration $k$ .

The measurement equation is given by:

${\bf z}_k = {\bf H} {\bf x}_k + {\bf r}_k$

where ${\bf z}_{k}$ is the measure (also named observation) at iteration $k$ .

The predicted state is obtained by:

${\bf x}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}$

${\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1} + {\bf Q}_k$

where

${\bf x}_{k|k-1}$ is the prediction of the state,
${\bf P}_{k \mid k-1}$ is the state prediction covariance matrix.

Filtering equation are:

${\bf W}_k = {\bf P}_{k \mid k-1} {\bf H}^T \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \right]^{-1}$

${\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k \left[ {\bf z}_k - {\bf H x}_{k \mid k-1} \right]$

${\bf P}_{k \mid k} = \left({\bf I - W}_k {\bf H} \right) {\bf P}_{k \mid k-1}$

where ${\bf W}_k$ is the filter gain.

Notice that there is a recursion for the inverse covariance

${\bf P}_{k \mid k}^{-1}= {\bf P}_{k \mid k-1}^{-1} + {\bf H}^T {\bf R}^{-1} {\bf H}$

where ${\bf P}_{k \mid k}^{-1}$ is the inverse of the covariance matrix.

ViSP provides different state evolution models implemented in the vpLinearKalmanFilterInstantiation class.

Definition at line 104 of file vpKalmanFilter.h.

Constructor & Destructor Documentation

◆ vpKalmanFilter() [1/3]

vpKalmanFilter::vpKalmanFilter ( )

Construct a default Kalman filter.

The verbose mode is disabled by default

Definition at line 88 of file vpKalmanFilter.cpp.

◆ vpKalmanFilter() [2/3]

vpKalmanFilter::vpKalmanFilter ( unsigned int n_signal )

explicit

Construct a default Kalman filter by setting the number of signal to filter.

The verbose mode is disabled by default

Parameters

n_signal : Number of signal to filter.

Definition at line 101 of file vpKalmanFilter.cpp.

◆ vpKalmanFilter() [3/3]

vpKalmanFilter::vpKalmanFilter	(	unsigned int	size_state_vector,
		unsigned int	size_measure_vector,
		unsigned int	n_signal
	)

Construct a Kalman filter.

The verbose mode is disabled by default

Parameters

size_state_vector	: Size of the state vector ${\bf x}_{(k)}$ for one signal.
size_measure_vector	: Size of the measure vector ${\bf z}_{(k)}$ for one signal.
n_signal	: Number of signal to filter.

Definition at line 120 of file vpKalmanFilter.cpp.

References init().

◆ ~vpKalmanFilter()

virtual vpKalmanFilter::~vpKalmanFilter ( )

inlinevirtual

Destructor that does noting.

Definition at line 128 of file vpKalmanFilter.h.

Member Function Documentation

◆ filtering()

void vpKalmanFilter::filtering ( const vpColVector & z )

Update the Kalman filter by applying the filtering equations and increment the filter iteration (vpKalmanFilter::iter).

Parameters

z	: Measure (or observation) ${\bf z}_k$ provided at iteration .

The filtering equation is given by:

${\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k \left[ {\bf z}_k - {\bf H} {\bf x}_{k \mid k-1} \right]$

where ${\bf W_k}$ is the filter gain computed using the formula:

${\bf W_k} = {\bf P}_{k \mid k-1} {\bf H}^T \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \right]^{-1}$

and where the updated covariance of the state is given by

${\bf P}_{k \mid k} = \left({\bf I} - {\bf W}_k {\bf H} \right) {\bf P}_{k \mid k-1}$

or in a symmetric form

${\bf P}_{k \mid k} = {\bf P}_{k \mid k-1} - {\bf W}_k {\bf S}_k {\bf W}^T_k$

with

${\bf S}_k = {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k$

Definition at line 200 of file vpKalmanFilter.cpp.

References H, vpMatrix::inverseByLU(), iter, Pest, Ppre, R, vpMatrix::t(), verbose_mode, W, Xest, and Xpre.

Referenced by vpLinearKalmanFilterInstantiation::filter().

◆ getIteration()

long vpKalmanFilter::getIteration ( )

inline

Return the iteration number.

Definition at line 153 of file vpKalmanFilter.h.

◆ getMeasureSize()

unsigned int vpKalmanFilter::getMeasureSize ( )

inline

Return the size of the measure vector ${\bf z}_{(k)}$ for one signal.

Examples: testKalmanAcceleration.cpp.

Definition at line 145 of file vpKalmanFilter.h.

◆ getNumberOfSignal()

unsigned int vpKalmanFilter::getNumberOfSignal ( )

inline

Return the number of signal to filter.

Definition at line 149 of file vpKalmanFilter.h.

◆ getStateSize()

unsigned int vpKalmanFilter::getStateSize ( )

inline

Return the size of the state vector ${\bf x}_{(k)}$ for one signal.

Examples: servoAfma4Point2DCamVelocityKalman.cpp, servoViper850Point2DCamVelocityKalman.cpp, and testKalmanAcceleration.cpp.

Definition at line 141 of file vpKalmanFilter.h.

◆ init()

void vpKalmanFilter::init	(	unsigned int	size_state_vector,
		unsigned int	size_measure_vector,
		unsigned int	n_signal
	)

Initialize the Kalman filter.

Parameters

size_state_vector	: Size of the state vector ${\bf x}_{k}$ for one signal.
size_measure_vector	: Size of the measure vector ${\bf z}_{k}$ for one signal.
n_signal	: Number of signal to filter.

Definition at line 57 of file vpKalmanFilter.cpp.

References dt, F, H, I, iter, nsignal, Pest, Q, R, vpColVector::resize(), vpArray2D< Type >::resize(), size_measure, size_state, Xest, and Xpre.

Referenced by vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and vpKalmanFilter().

◆ prediction()

void vpKalmanFilter::prediction ( )

Update the Kalman filter by applying the prediction equations.

The predicted state is given by

${{\bf x}}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}$

and the state prediction covariance by

${\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1} + {\bf Q}_k$

Definition at line 143 of file vpKalmanFilter.cpp.

References F, vpException::fatalError, vpArray2D< Type >::getRows(), nsignal, Pest, Ppre, Q, size_state, vpMatrix::t(), verbose_mode, Xest, and Xpre.

Referenced by vpLinearKalmanFilterInstantiation::filter().

◆ setNumberOfSignal()

void vpKalmanFilter::setNumberOfSignal ( unsigned int n_signal )

inline

Set the number of signal to filter.

Definition at line 132 of file vpKalmanFilter.h.

◆ verbose()

void vpKalmanFilter::verbose ( bool on )

inline

Sets the verbose mode.

Parameters

on	: If true, activates the verbose mode which consists in printing the Kalman filter internal values.

Examples: testKalmanAcceleration.cpp, and testKalmanVelocity.cpp.

Definition at line 159 of file vpKalmanFilter.h.

Member Data Documentation

◆ dt

double vpKalmanFilter::dt

Sampling time $\Delta t$ in second between two successive iterations. Only used in some specific state models implemented in vpLinearKalmanFilterInstantiation.

Definition at line 187 of file vpKalmanFilter.h.

Referenced by vpLinearKalmanFilterInstantiation::filter(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), and vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos().

◆ F

vpMatrix vpKalmanFilter::F

Transition matrix ${\bf F}$ that describes the evolution of the state.

Definition at line 175 of file vpKalmanFilter.h.

Referenced by init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and prediction().

◆ H

vpMatrix vpKalmanFilter::H

Matrix ${\bf H}$ that describes the evolution of the measurements.

Definition at line 178 of file vpKalmanFilter.h.

Referenced by filtering(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), and vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel().

◆ I

vpMatrix vpKalmanFilter::I

protected

Identity matrix $\bf I$ .

Definition at line 212 of file vpKalmanFilter.h.

Referenced by init().

◆ iter

long vpKalmanFilter::iter

protected

Filter step or iteration. When set to zero, initialize the filter.

Definition at line 110 of file vpKalmanFilter.h.

Referenced by vpLinearKalmanFilterInstantiation::filter(), filtering(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), and vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel().

◆ nsignal

unsigned int vpKalmanFilter::nsignal

protected

Number of signal to filter.

Definition at line 117 of file vpKalmanFilter.h.

Referenced by vpLinearKalmanFilterInstantiation::filter(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), and prediction().

◆ Pest

vpMatrix vpKalmanFilter::Pest

The updated covariance of the state ${\bf P}_{k \mid k}$ where ${\bf P}_{k \mid k} = \left({\bf I - W}_k {\bf H} \right) {\bf P}_{k \mid k-1}$ .

Definition at line 201 of file vpKalmanFilter.h.

Referenced by filtering(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and prediction().

◆ Ppre

vpMatrix vpKalmanFilter::Ppre

The state prediction covariance ${\bf P}_{k \mid k-1}$ where ${\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1} + {\bf Q}_k$ .

Definition at line 194 of file vpKalmanFilter.h.

Referenced by filtering(), and prediction().

◆ Q

vpMatrix vpKalmanFilter::Q

Process noise covariance matrix ${\bf Q}$ .

Definition at line 183 of file vpKalmanFilter.h.

Referenced by init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and prediction().

◆ R

vpMatrix vpKalmanFilter::R

Measurement noise covariance matrix ${\bf R}$ .

Definition at line 181 of file vpKalmanFilter.h.

Referenced by filtering(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), and vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel().

◆ size_measure

unsigned int vpKalmanFilter::size_measure

protected

Size of the measure vector ${\bf z}_k$ .

Definition at line 115 of file vpKalmanFilter.h.

Referenced by vpLinearKalmanFilterInstantiation::filter(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and vpLinearKalmanFilterInstantiation::setStateModel().

◆ size_state

unsigned int vpKalmanFilter::size_state

protected

Size of the state vector ${\bf x}_k$ .

Definition at line 113 of file vpKalmanFilter.h.

Referenced by vpLinearKalmanFilterInstantiation::filter(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), prediction(), and vpLinearKalmanFilterInstantiation::setStateModel().

◆ verbose_mode

bool vpKalmanFilter::verbose_mode

protected

When set to true, print the content of internal variables during filtering() and prediction().

Definition at line 121 of file vpKalmanFilter.h.

Referenced by filtering(), and prediction().

◆ W

vpMatrix vpKalmanFilter::W

protected

Filter gain ${\bf W}_k$ where ${\bf W}_k = {\bf P}_{k \mid k-1} {\bf H}^T \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf R}_k \right]^{-1}$ .

Definition at line 209 of file vpKalmanFilter.h.

Referenced by filtering().

◆ Xest

vpColVector vpKalmanFilter::Xest

The updated state estimate ${\bf x}_{k \mid k}$ where ${\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k \left[ {\bf z}_k - {\bf H x}_{k \mid k-1} \right]$ .