Visual Servoing Platform  version 3.6.1 under development (2024-11-21)
vpKalmanFilter Class Reference

#include <visp3/core/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)
 
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 105 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 89 of file vpKalmanFilter.cpp.

◆ vpKalmanFilter() [2/3]

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

References init().

◆ ~vpKalmanFilter()

virtual vpKalmanFilter::~vpKalmanFilter ( )
inlinevirtual

Destructor that does noting.

Definition at line 129 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 $k$.

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 199 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 154 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 146 of file vpKalmanFilter.h.

◆ getNumberOfSignal()

unsigned int vpKalmanFilter::getNumberOfSignal ( )
inline

Return the number of signal to filter.

Definition at line 150 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 142 of file vpKalmanFilter.h.

◆ 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 58 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 142 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 133 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 160 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 188 of file vpKalmanFilter.h.

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

◆ F

◆ H

◆ I

vpMatrix vpKalmanFilter::I
protected

Identity matrix $ \bf I$.

Definition at line 213 of file vpKalmanFilter.h.

Referenced by init().

◆ iter

◆ nsignal

unsigned int vpKalmanFilter::nsignal
protected

◆ Pest

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

Referenced by filtering(), and prediction().

◆ Q

◆ R

◆ size_measure

◆ size_state

◆ 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(), 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 210 of file vpKalmanFilter.h.

Referenced by filtering().

◆ Xest

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

Examples
testKalmanAcceleration.cpp.

Definition at line 173 of file vpKalmanFilter.h.

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