Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpKalmanFilter Class Reference

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

Constructor & Destructor Documentation

vpKalmanFilter::vpKalmanFilter ( )

Construct a default Kalman filter.

The verbose mode is by default desactivated.

Definition at line 90 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 103 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 121 of file vpKalmanFilter.cpp.

References init().

virtual vpKalmanFilter::~vpKalmanFilter ( )
inlinevirtual

Destructor that does noting.

Definition at line 131 of file vpKalmanFilter.h.

Member Function Documentation

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

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

Referenced by vpLinearKalmanFilterInstantiation::filter().

long vpKalmanFilter::getIteration ( )
inline

Return the iteration number.

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

unsigned int vpKalmanFilter::getNumberOfSignal ( )
inline

Return the number of signal to filter.

Definition at line 155 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 147 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 61 of file vpKalmanFilter.cpp.

References dt, F, H, I, iter, nsignal, Pest, Q, R, vpArray2D< Type >::resize(), vpColVector::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 144 of file vpKalmanFilter.cpp.

References F, vpArray2D< Type >::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 135 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 165 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 192 of file vpKalmanFilter.h.

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

vpMatrix vpKalmanFilter::I
protected

Identity matrix $ \bf I$.

Definition at line 217 of file vpKalmanFilter.h.

Referenced by init().

unsigned int vpKalmanFilter::nsignal
protected
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 198 of file vpKalmanFilter.h.

Referenced by filtering(), and prediction().

bool vpKalmanFilter::verbose_mode
protected

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

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

Referenced by filtering().

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

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