Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
Public Member Functions | Public Attributes | Protected Attributes | List of all members
vpKalmanFilter Class Reference
core: Core module » Mathematics » Kalman filter

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

Constructor & Destructor Documentation

◆ vpKalmanFilter() [1/3]

vpKalmanFilter::vpKalmanFilter ( )

Construct a default Kalman filter.

The verbose mode is by default desactivated.

Definition at line 92 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 by default desactivated.

Parameters
n_signal: Number of signal to filter.

Definition at line 105 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 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().

◆ ~vpKalmanFilter()

virtual vpKalmanFilter::~vpKalmanFilter ( )
inlinevirtual

Destructor that does noting.

Definition at line 134 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 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 211 of file vpKalmanFilter.cpp.

References dt, F, H, vpMatrix::inverseByLU(), iter, Pest, Ppre, Q, R, size_measure, vpMatrix::t(), 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.

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

◆ getNumberOfSignal()

unsigned int vpKalmanFilter::getNumberOfSignal ( )
inline

Return the number of signal to filter.

Definition at line 155 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, and servoViper850Point2DCamVelocityKalman.cpp.

Definition at line 147 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 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().

◆ 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 147 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().

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

Member Data Documentation

◆ dt

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

Referenced by vpLinearKalmanFilterInstantiation::filter(), filtering(), 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 181 of file vpKalmanFilter.h.

Referenced by filtering(), 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 184 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 218 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 116 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 123 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 207 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 200 of file vpKalmanFilter.h.

Referenced by filtering(), and prediction().

◆ Q

vpMatrix vpKalmanFilter::Q

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

Definition at line 189 of file vpKalmanFilter.h.

Referenced by filtering(), 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 187 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 121 of file vpKalmanFilter.h.

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

◆ size_state

unsigned int vpKalmanFilter::size_state
protected

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

Definition at line 119 of file vpKalmanFilter.h.

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

◆ verbose_mode

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().

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

Examples:
servoAfma4Point2DCamVelocityKalman.cpp, and servoViper850Point2DCamVelocityKalman.cpp.

Definition at line 165 of file vpKalmanFilter.h.

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

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

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