#include <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 (vpColVector &z)

unsigned int	getStateSize ()

unsigned int	getMeasureSize ()

unsigned int	getNumberOfSignal ()

int	getIteration ()

void	verbose (bool on)

Public Attributes
vpColVector	Xest

vpColVector	Xpre

vpMatrix	F

vpMatrix	H

vpMatrix	R

vpMatrix	Q

double	dt

Protected Attributes
long	iter

unsigned int	size_state

unsigned int	size_measure

unsigned int	nsignal

bool	verbose_mode

vpMatrix	Ppre

vpMatrix	Pest

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 111 of file vpKalmanFilter.h.

Constructor & Destructor Documentation

vpKalmanFilter::vpKalmanFilter ( )

Construct a default Kalman filter.

The verbose mode is by default desactivated.

Definition at line 93 of file vpKalmanFilter.cpp.

vpKalmanFilter::vpKalmanFilter ( unsigned int n_signal )

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

The verbose mode is by default desactivated.

Parameters

n_signal : Number of signal to filter.

Definition at line 106 of file vpKalmanFilter.cpp.

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

Construct a Kalman filter.

The verbose mode is by default desactivated.

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 124 of file vpKalmanFilter.cpp.

References init().

virtual vpKalmanFilter::~vpKalmanFilter ( )

inlinevirtual

Destructor that does noting.

Definition at line 134 of file vpKalmanFilter.h.

Member Function Documentation

void vpKalmanFilter::filtering ( 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 symetric 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 215 of file vpKalmanFilter.cpp.

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

Referenced by vpLinearKalmanFilterInstantiation::filter().

int vpKalmanFilter::getIteration ( )

inline

Return the iteration number.

Definition at line 162 of file vpKalmanFilter.h.

unsigned int vpKalmanFilter::getMeasureSize ( )

inline

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

Definition at line 154 of file vpKalmanFilter.h.

unsigned int vpKalmanFilter::getNumberOfSignal ( )

inline

Return the number of signal to filter.

Definition at line 158 of file vpKalmanFilter.h.

unsigned int vpKalmanFilter::getStateSize ( )

inline

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

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

Definition at line 150 of file vpKalmanFilter.h.

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 64 of file vpKalmanFilter.cpp.

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

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

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 147 of file vpKalmanFilter.cpp.

References F, vpMatrix::getRows(), nsignal, Pest, Ppre, Q, size_state, vpMatrix::t(), verbose_mode, Xest, and Xpre.

Referenced by vpLinearKalmanFilterInstantiation::filter().

void vpKalmanFilter::setNumberOfSignal ( unsigned int n_signal )

inline

Set the number of signal to filter.

Definition at line 138 of file vpKalmanFilter.h.

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.

Definition at line 168 of file vpKalmanFilter.h.

Member Data Documentation

double vpKalmanFilter::dt

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

Definition at line 195 of file vpKalmanFilter.h.

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

vpMatrix vpKalmanFilter::F

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

Definition at line 183 of file vpKalmanFilter.h.

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

vpMatrix vpKalmanFilter::H

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

Definition at line 186 of file vpKalmanFilter.h.

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

vpMatrix vpKalmanFilter::I

protected

Identity matrix $\bf I$ .

Definition at line 220 of file vpKalmanFilter.h.

Referenced by init().

long vpKalmanFilter::iter

protected

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

Definition at line 117 of file vpKalmanFilter.h.

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

unsigned int vpKalmanFilter::nsignal

protected

Number of signal to filter.

Definition at line 124 of file vpKalmanFilter.h.

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

vpMatrix vpKalmanFilter::Pest

protected

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 210 of file vpKalmanFilter.h.

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

vpMatrix vpKalmanFilter::Ppre

protected

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 203 of file vpKalmanFilter.h.

Referenced by filtering(), and prediction().

vpMatrix vpKalmanFilter::Q

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

Definition at line 191 of file vpKalmanFilter.h.

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

vpMatrix vpKalmanFilter::R

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

Definition at line 189 of file vpKalmanFilter.h.

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

unsigned int vpKalmanFilter::size_measure

protected

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

Definition at line 122 of file vpKalmanFilter.h.

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

unsigned int vpKalmanFilter::size_state

protected

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

Definition at line 120 of file vpKalmanFilter.h.

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

bool vpKalmanFilter::verbose_mode

protected

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

Definition at line 127 of file vpKalmanFilter.h.

Referenced by filtering(), and prediction().

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 217 of file vpKalmanFilter.h.

Referenced by filtering().

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]$ .

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

Definition at line 168 of file vpKalmanFilter.h.

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

vpColVector vpKalmanFilter::Xpre

The predicted state ${\bf x}_{k \mid k-1}$ where ${\bf x}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}$ .

Definition at line 181 of file vpKalmanFilter.h.

Referenced by filtering(), init(), and prediction().

Public Member Functions

Public Attributes

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation