#include <vpKalmanFilter.h>

Inheritance diagram for vpKalmanFilter:

Public Member Functions
	vpKalmanFilter ()
VP_EXPLICIT	vpKalmanFilter (unsigned int n_signal)
	vpKalmanFilter (unsigned int size_state, unsigned int size_measure, unsigned int n_signal)
	vpKalmanFilter (const vpKalmanFilter &)=default
virtual	~vpKalmanFilter ()=default
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 ()
vpKalmanFilter &	operator= (const vpKalmanFilter &)=default
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 .

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 .

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

Constructor & Destructor Documentation

◆ vpKalmanFilter() [1/4]

vpKalmanFilter::vpKalmanFilter ( )

Construct a default Kalman filter.

The verbose mode is disabled by default

Definition at line 87 of file vpKalmanFilter.cpp.

References dt, F, H, I, iter, nsignal, Pest, Ppre, Q, R, size_measure, size_state, verbose_mode, W, Xest, and Xpre.

Referenced by operator=(), vpKalmanFilter(), and ~vpKalmanFilter().

◆ vpKalmanFilter() [2/4]

vpKalmanFilter::vpKalmanFilter ( unsigned int n_signal )

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

References dt, F, H, I, iter, nsignal, Pest, Ppre, Q, R, size_measure, size_state, verbose_mode, W, Xest, and Xpre.

◆ vpKalmanFilter() [3/4]

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

References dt, F, H, I, init(), iter, nsignal, Pest, Ppre, Q, R, size_measure, size_state, verbose_mode, W, Xest, and Xpre.

◆ vpKalmanFilter() [4/4]

vpKalmanFilter::vpKalmanFilter ( const vpKalmanFilter & )

default

Destructor that does noting.

References vpKalmanFilter().

◆ ~vpKalmanFilter()

virtual vpKalmanFilter::~vpKalmanFilter ( )

virtualdefault

References vpKalmanFilter().

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

References H, iter, Pest, Ppre, R, verbose_mode, W, Xest, and Xpre.

Referenced by vpLinearKalmanFilterInstantiation::filter().

◆ getIteration()

long vpKalmanFilter::getIteration ( )

inline

Return the iteration number.

Definition at line 159 of file vpKalmanFilter.h.

References iter.

◆ getMeasureSize()

unsigned int vpKalmanFilter::getMeasureSize ( )

inline

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

Definition at line 151 of file vpKalmanFilter.h.

References size_measure.

◆ getNumberOfSignal()

unsigned int vpKalmanFilter::getNumberOfSignal ( )

inline

Return the number of signal to filter.

Definition at line 155 of file vpKalmanFilter.h.

References nsignal.

◆ getStateSize()

unsigned int vpKalmanFilter::getStateSize ( )

inline

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

Definition at line 147 of file vpKalmanFilter.h.

References size_state.

◆ init()

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

References dt, F, H, I, iter, nsignal, Pest, Q, R, size_measure, size_state, Xest, and Xpre.

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

◆ operator=()

vpKalmanFilter & vpKalmanFilter::operator= ( const vpKalmanFilter & )

default

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

References F, vpException::fatalError, nsignal, Pest, Ppre, Q, size_state, 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 138 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.

Definition at line 169 of file vpKalmanFilter.h.

References verbose_mode.

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

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

◆ F

vpMatrix vpKalmanFilter::F

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

Definition at line 185 of file vpKalmanFilter.h.

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

◆ H

vpMatrix vpKalmanFilter::H

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

Definition at line 188 of file vpKalmanFilter.h.

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

◆ I

vpMatrix vpKalmanFilter::I

protected

Identity matrix $\bf I$ .

Definition at line 222 of file vpKalmanFilter.h.

Referenced by yolo-centering-task-afma6.VSPlot::generate_anim(), yolo-centering-task.VSPlot::generate_anim(), init(), yolo-centering-task-afma6.VSPlot::on_iter(), yolo-centering-task.VSPlot::on_iter(), vpKalmanFilter(), vpKalmanFilter(), and vpKalmanFilter().

◆ iter

long vpKalmanFilter::iter

protected

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

Definition at line 111 of file vpKalmanFilter.h.

Referenced by visp.python.rbt.xfeat.XFeatVisualOdometry.XFeatVisualOdometry::compute(), vpLinearKalmanFilterInstantiation::filter(), filtering(), getIteration(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), visp.python.rbt.xfeat.RBXFeatFeatureTracker.RBXFeatFeatureTracker::reset(), visp.python.rbt.xfeat.XFeatVisualOdometry.XFeatVisualOdometry::reset(), vpKalmanFilter(), vpKalmanFilter(), and vpKalmanFilter().

◆ nsignal

unsigned int vpKalmanFilter::nsignal

protected

Number of signal to filter.

Definition at line 118 of file vpKalmanFilter.h.

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

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

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

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

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

◆ Q

vpMatrix vpKalmanFilter::Q

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

Definition at line 193 of file vpKalmanFilter.h.

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

◆ R

vpMatrix vpKalmanFilter::R

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

Definition at line 191 of file vpKalmanFilter.h.

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

◆ size_measure

unsigned int vpKalmanFilter::size_measure

protected

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

Definition at line 116 of file vpKalmanFilter.h.

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

◆ size_state

unsigned int vpKalmanFilter::size_state

protected

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

Definition at line 114 of file vpKalmanFilter.h.

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

◆ verbose_mode

bool vpKalmanFilter::verbose_mode

protected

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

Definition at line 122 of file vpKalmanFilter.h.

Referenced by filtering(), prediction(), verbose(), vpKalmanFilter(), vpKalmanFilter(), and vpKalmanFilter().

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

Referenced by filtering(), vpKalmanFilter(), vpKalmanFilter(), and vpKalmanFilter().

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

Definition at line 177 of file vpKalmanFilter.h.

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

◆ Xpre

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

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

Public Member Functions

Public Attributes

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ vpKalmanFilter() [1/4]

◆ vpKalmanFilter() [2/4]

◆ vpKalmanFilter() [3/4]

◆ vpKalmanFilter() [4/4]

◆ ~vpKalmanFilter()

Member Function Documentation

◆ filtering()

◆ getIteration()

◆ getMeasureSize()

◆ getNumberOfSignal()

◆ getStateSize()

◆ init()

◆ operator=()

◆ prediction()

◆ setNumberOfSignal()

◆ verbose()

Member Data Documentation

◆ dt

◆ F

◆ H

◆ I

◆ iter

◆ nsignal

◆ Pest

◆ Ppre

◆ Q

◆ R

◆ size_measure

◆ size_state

◆ verbose_mode

◆ W

◆ Xest

◆ Xpre