Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
Public Types | Public Member Functions | Public Attributes | Protected Attributes | List of all members
vpLinearKalmanFilterInstantiation Class Reference
core: Core module » Mathematics » Kalman filter

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>

+ Inheritance diagram for vpLinearKalmanFilterInstantiation:

Public Types

enum  vpStateModel { stateConstVel_MeasurePos, stateConstVelWithColoredNoise_MeasureVel, stateConstAccWithColoredNoise_MeasureVel, unknown }
 

Public Member Functions

 vpLinearKalmanFilterInstantiation ()
 
virtual ~vpLinearKalmanFilterInstantiation ()
 
vpStateModel getStateModel ()
 
void filter (vpColVector &z)
 
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)
 
Generic linear filter initializer
void setStateModel (vpStateModel model)
 
void initFilter (unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
 
Linear filter initializer with constant velocity models
void initStateConstVel_MeasurePos (unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double dt)
 
void initStateConstVelWithColoredNoise_MeasureVel (unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho)
 
Linear filter initializer with constant acceleration models
void initStateConstAccWithColoredNoise_MeasureVel (unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
 

Public Attributes

vpColVector Xest
 
vpColVector Xpre
 
vpMatrix F
 
vpMatrix H
 
vpMatrix R
 
vpMatrix Q
 
double dt
 
vpMatrix Ppre
 
vpMatrix Pest
 

Protected Attributes

vpStateModel model
 
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 an implementation of some specific linear Kalman filters.

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

Definition at line 58 of file vpLinearKalmanFilterInstantiation.h.

Member Enumeration Documentation

◆ vpStateModel

enum vpLinearKalmanFilterInstantiation::vpStateModel

Selector used to set the Kalman filter state model.

Enumerator
stateConstVel_MeasurePos 

Consider the state as a constant velocity model with white noise. Measures available are the successive positions of the target. To know more about this state model, see initStateConstVel_MeasurePos().

stateConstVelWithColoredNoise_MeasureVel 

Consider the state as a constant velocity model with colored noise measurements as acceleration terms. Measured available are the velocities of the target. To know more about this state model, see initStateConstVelWithColoredNoise_MeasureVel().

stateConstAccWithColoredNoise_MeasureVel 

Consider the state as a constant acceleration model with colored noise measurements as acceleration terms. Measured available are the velocities of the target. To know more about this state model, see initStateConstAccWithColoredNoise_MeasureVel().

unknown 

Used to indicate that the state model is not initialized.

Definition at line 64 of file vpLinearKalmanFilterInstantiation.h.

Constructor & Destructor Documentation

◆ vpLinearKalmanFilterInstantiation()

vpLinearKalmanFilterInstantiation::vpLinearKalmanFilterInstantiation ( )
inline

Default linear Kalman filter.

By default the state model is unknown and set to vpLinearKalmanFilterInstantiation::unknown.

Definition at line 90 of file vpLinearKalmanFilterInstantiation.h.

◆ ~vpLinearKalmanFilterInstantiation()

virtual vpLinearKalmanFilterInstantiation::~vpLinearKalmanFilterInstantiation ( )
inlinevirtual

Destructor that does nothng.

Definition at line 93 of file vpLinearKalmanFilterInstantiation.h.

Member Function Documentation

◆ filter()

void vpLinearKalmanFilterInstantiation::filter ( vpColVector &  z)

Do the filtering and prediction of the measure signal.

Parameters
z: Measures ${\bf z}_k$ used to initialise the filter. The dimension of this vector is equal to the number of signal to filter (given by getNumberOfSignal()) multiplied by the size of the measure vector (given by getMeasureSize()) .
Exceptions
vpException::notInitialized: If the filter is not initialized. To initialize the filter see initFilter().
Examples:
servoAfma4Point2DCamVelocityKalman.cpp, and servoViper850Point2DCamVelocityKalman.cpp.

Definition at line 790 of file vpLinearKalmanFilterInstantiation.cpp.

References vpKalmanFilter::dt, vpKalmanFilter::filtering(), vpKalmanFilter::iter, model, vpException::notInitialized, vpKalmanFilter::nsignal, vpKalmanFilter::prediction(), vpKalmanFilter::size_measure, vpKalmanFilter::size_state, stateConstAccWithColoredNoise_MeasureVel, stateConstVel_MeasurePos, stateConstVelWithColoredNoise_MeasureVel, unknown, vpERROR_TRACE, and vpKalmanFilter::Xest.

◆ filtering()

void vpKalmanFilter::filtering ( const vpColVector &  z)
inherited

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

References vpKalmanFilter::dt, vpKalmanFilter::F, vpKalmanFilter::H, vpMatrix::inverseByLU(), vpKalmanFilter::iter, vpKalmanFilter::Pest, vpKalmanFilter::Ppre, vpKalmanFilter::Q, vpKalmanFilter::R, vpKalmanFilter::size_measure, vpMatrix::t(), vpKalmanFilter::verbose_mode, vpKalmanFilter::W, vpKalmanFilter::Xest, and vpKalmanFilter::Xpre.

Referenced by filter().

◆ getIteration()

long vpKalmanFilter::getIteration ( )
inlineinherited

Return the iteration number.

Definition at line 159 of file vpKalmanFilter.h.

◆ getMeasureSize()

unsigned int vpKalmanFilter::getMeasureSize ( )
inlineinherited

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

Return the number of signal to filter.

Definition at line 155 of file vpKalmanFilter.h.

◆ getStateModel()

vpStateModel vpLinearKalmanFilterInstantiation::getStateModel ( )
inline

Return the current state model.

Definition at line 97 of file vpLinearKalmanFilterInstantiation.h.

◆ getStateSize()

unsigned int vpKalmanFilter::getStateSize ( )
inlineinherited

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 
)
inherited

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 vpKalmanFilter::dt, vpKalmanFilter::F, vpKalmanFilter::H, vpKalmanFilter::I, vpKalmanFilter::iter, vpKalmanFilter::nsignal, vpKalmanFilter::Pest, vpKalmanFilter::Q, vpKalmanFilter::R, vpArray2D< Type >::resize(), vpColVector::resize(), vpKalmanFilter::size_measure, vpKalmanFilter::size_state, vpKalmanFilter::Xest, and vpKalmanFilter::Xpre.

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

◆ initFilter()

void vpLinearKalmanFilterInstantiation::initFilter ( unsigned int  n_signal,
vpColVector &  sigma_state,
vpColVector &  sigma_measure,
double  rho,
double  delta_t 
)

Initialize the Kalman filter material depending on the selected state model set with setStateModel(). This function is provided as a wrapper over all the other initializer functions like initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), initStateConstAccWithColoredNoise_MeasureVel().

Warning
It is requiered to set the state model before using this method.
Parameters
n_signal: Number of signal to filter.
sigma_state: Vector that contains the variance of the state noise. The dimension of this vector is equal to the state vector ${\bf x}$ size multiplied by the number of signal to filter. Values are used to initialize the ${\bf Q}$ state covariance matrix.
sigma_measure: Vector that contains the variance of the measurement noise. The dimension of this vector is equal to the measure vector ${\bf x}$ size multiplied by the number of signal to filter. Values are used to initialize the ${\bf R}$ measure covariance matrix.
rho: Degree of correlation between successive accelerations. Values are in [0:1[.
delta_t: Sampling time $\Delta t$ expressed is second. Depending on the filter modelization, this value may not be used. This is for example the case for the vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel model implemented in initStateConstVelWithColoredNoise_MeasureVel().
Exceptions
vpException::badValue: Bad rho value wich is not in [0:1[.
vpException::notInitialized: If the state model is not initialized. To initialize it you need to call setStateModel().

The example below shows how to initialize the filter for a one dimensional signal.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
int main()
{
vpLinearKalmanFilterInstantiation kalman;
// Select a constant velocity state model with colored noise
// Measures are velocities
kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);
// Initialise the filter for a one dimension signal
int signal = 1;
vpColVector sigma_state(2); // State vector size is 2
vpColVector sigma_measure(1); // Measure vector size is 1
double rho = 0.9;
double dummy = 0; // non used parameter for the selected state model
kalman.initFilter(signal, sigma_state, sigma_measure, rho, dummy);
}

The example below shows a more complete example to filter a two dimensional target trajectory with an estimation of the target velocities from velocity measures.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
int main()
{
vpLinearKalmanFilterInstantiation kalman;
// Set the constant velocity state model used for the filtering
vpLinearKalmanFilterInstantiation::vpStateModel model;
model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel;
kalman.setStateModel(model);
// We are now able to retrieve the size of the state vector and measure vector
int state_size = kalman.getStateSize();
// Filter the x and y velocities of a target (2 signals are to consider)
int nsignal = 2;
// Initialize the filter parameters:
// - Firstly, the state variance
int size = state_size*nsignal;
vpColVector sigma_state(size);
sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
// - Secondly, the measures variance
vpColVector sigma_measure(nsignal); // 2 velocity measures available
sigma_measure[0] = 0.03; // Variance on the x velocity measure
sigma_measure[1] = 0.06; // Variance on the y velocity measure
// - Thirdly, the correlation between successive accelerations
double rho = 0.9;
double dummy = 0; // non used parameter for the selected state model
// Initialize the filter
// The state model was set before
kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
// Does the filtering
vpColVector vm(2); // Measured velocities
for ( ; ; ) {
// Get the two dimentional velocity measures
// vm[0] = ...;
// vm[1] = ...;
// Compute the filtering and the prediction
kalman.filter(vm);
// Print the estimation of the velocities (1st value of the state vector)
std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
<< std::endl;
// The one step prediction is available in kalman.Xpre variable
}
}
Examples:
servoAfma4Point2DCamVelocityKalman.cpp, and servoViper850Point2DCamVelocityKalman.cpp.

Definition at line 174 of file vpLinearKalmanFilterInstantiation.cpp.

References initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), model, vpException::notInitialized, stateConstAccWithColoredNoise_MeasureVel, stateConstVel_MeasurePos, stateConstVelWithColoredNoise_MeasureVel, unknown, and vpERROR_TRACE.

◆ initStateConstAccWithColoredNoise_MeasureVel()

void vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel ( unsigned int  n_signal,
vpColVector &  sigma_state,
vpColVector &  sigma_measure,
double  rho,
double  delta_t 
)

Modelisation of a constant acceleration state model with colored noise. The measure is assumed to be the velocity of the target.

This state model assume that there is some memory associated with noise measurements as acceleration terms. They can be represented as remaining correlated (or colored) over successive time intervals, leading to the following state model:

\[ \left\{ \begin{array}{rll} x_{(k+1)} & = x_{(k)} + \Delta t \; \dot{x}_{(k)} + \nu_{(k)} &\\ \nu_{(k+1)}& = \rho \nu_{(k)} &+w_{1(k)} \\ \dot{x}_{(k+1)} & = \dot{x}_{(k)} &+w_{2(k)}\\ \end{array} \right. \]

The terms $w_{1(k)}$ and $w_{2(k)}$ account for deviations from the assumed constant acceleration trajectory. They are assumed zero-mean, white, mutually uncorrelated, stationary random variable with variance $\sigma^2_{Q_1}$ and $\sigma^2_{Q_2}$. The term $\rho$ is the degree of correlation between successive accelerations. Values can range from 0 to 1.

We recall that the recursive state evolution equation is given by

\[ {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\ \]

From this state model, the transition matrix ${\bf F}$ and the state covariance matrix ${\bf Q}$ are given by:

\[ {\bf F} = \left[ \begin{array}{ccc} 1 & 1 & \Delta t\\ 0 & \rho & 0 \\ 0 & 0 & 1 \end{array} \right] \]

and

\[ {\bf Q} = \left[ \begin{array}{ccc} 0 & 0 & 0\\ 0 & \sigma^2_{Q_1} & 0\\ 0 & 0& \sigma^2_{Q_2} \\ \end{array} \right] \]

The measurement model is given by:

\[ z_{(k)} = {\bf H} {\bf x}_{(k)} + r_{(k)} \]

where ${\bf H} = [1 \; 0 \; 0]$, $z_{(k)}$ is the measure of the velocity and $r_{(k)}$ is the measurement noise, assumed zero-mean, white mutually uncorrelated stationary random variables with variance $\sigma^2_R$, giving the covariance matrix:

\[ {\bf R} = \left[\sigma^2_R\right] \]

The initial value of the state vector is set to:

\[ {\bf x_{(0)}} = \left[ \begin{array}{c} z_{(0)}\\ 0 \\ 0 \end{array} \right] \]

The initial value $P_{(0|0)}$ of the prediction covariance matrix is given by:

\[ {\bf P_{(0|0)}} = \left[ \begin{array}{ccc} \sigma^2_R & 0 & \sigma^2_R / \Delta t\\ 0 & \sigma^2_{Q_1}/(1-\rho^2) & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t \\ \sigma^2_R / \Delta t & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t & (2\sigma^2_R +\sigma^2_{Q_1}/(1-\rho^2) )/\Delta t^2 \end{array} \right] \]

Parameters
n_signal: Number of signal to filter.
sigma_state: Vector that fix the variance of the state covariance matrix $[0 \; \sigma^2_{Q_1} \; \sigma^2_{Q_2}]^T$. The dimension of this vector is 3 multiplied by the number of signal to filter.
sigma_measure: Variance $\sigma^2_R$ of the measurement noise. The dimension of this vector is equal to the number of signal to filter.
rho: Degree of correlation between successive accelerations. Values are in [0:1[.
delta_t: Sampling time $\Delta t$ expressed is second.
Exceptions
vpException::badValue: Bad rho value wich is not in [0:1[.

The example below shows how to filter a two dimensional target trajectory with an estimation of the target velocity from velocity measures.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
int main()
{
vpLinearKalmanFilterInstantiation kalman;
// Filter the x and y velocities of a target (2 signals are to consider)
int nsignal = 2;
// Initialize the filter parameters:
// - Firstly, the state variance
vpColVector sigma_state(6); // 6 = 3 for the state size x 2 signal
sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
sigma_state[2] = 0.001; // Variance on the acceleration for the 1st signal (x)
sigma_state[4] = 0.002; // Variance on the acceleration for the 2nd signal (y)
sigma_state[5] = 0.002; // Variance on the acceleration for the 2nd signal (y)
// - Secondly, the measures variance
vpColVector sigma_measure(nsignal); // 2 velocity measures available
sigma_measure[0] = 0.03; // Variance on the x velocity measure
sigma_measure[1] = 0.06; // Variance on the y velocity measure
// - Thirdly, the correlation between successive accelerations
double rho = 0.9;
// - Lastly, the sampling time
double dt = 0.020; // 20 ms
// Initialize the filter
kalman.initStateConstAccWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho, dt);
// Does the filtering
vpColVector vm(2); // Measured velocities
for ( ; ; ) {
// Get the two dimentional velocity measures
// vm[0] = ...;
// vm[1] = ...;
// Compute the filtering and the prediction
kalman.filter(vm);
// Print the estimation of the velocities (1st value of the state vector)
std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
<< std::endl;
// The one step prediction is available in kalman.Xpre variable
}
}

Definition at line 714 of file vpLinearKalmanFilterInstantiation.cpp.

References vpException::badValue, vpKalmanFilter::dt, vpKalmanFilter::F, vpKalmanFilter::H, vpKalmanFilter::init(), vpKalmanFilter::iter, vpKalmanFilter::nsignal, vpKalmanFilter::Pest, vpKalmanFilter::Q, vpKalmanFilter::R, setStateModel(), vpKalmanFilter::size_measure, vpKalmanFilter::size_state, stateConstAccWithColoredNoise_MeasureVel, vpERROR_TRACE, and vpKalmanFilter::Xest.

Referenced by initFilter().

◆ initStateConstVel_MeasurePos()

void vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos ( unsigned int  n_signal,
vpColVector &  sigma_state,
vpColVector &  sigma_measure,
double  delta_t 
)

Modelisation of a constant speed state model with white noise. The measure is assumed to be the position of the target.

The considered state model is the following

\[ \left\{ \begin{array}{rlrl} x_{(k+1)} & = x_{(k)} & + \Delta t \; {\dot{x}}_{(k)} & + {w_1}_{(k)} \\ {\dot{x}}_{(k+1)} & = & {\dot{x}}_{(k)} &+{w_2}_{(k)} \end{array} \right. \]

The terms ${w_1}_{(k)}$ and ${w_2}_{(k)}$ account for deviations from the assumed constant velocity trajectory. They are assumed zero-mean, white, mutually uncorrelated, stationary random variable with variance $\sigma^2_{Q_1}$ and $\sigma^2_{Q_2}$.

We recall that the recursive state evolution equation is given by

\[ {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\ \]

From this state model, the transition matrix ${\bf F}$ and the state covariance matrix ${\bf Q}$ are given by:

\[ {\bf F} = \left[ \begin{array}{cc} 1 & \Delta t\\ 0 & 1 \end{array} \right] \]

and

\[ {\bf Q} = \sigma^2_Q \left[ \begin{array}{cc} \frac{1}{3}\Delta t^3 & \frac{1}{2}\Delta t^2\\ \frac{1}{2}\Delta t^2 & \Delta t \end{array} \right] \]

The initial value of the state vector at iteration 0 is set to:

\[ {\bf x_{(0)}} = \left[ \begin{array}{c} z_{(0)}\\ 0 \end{array} \right] \]

The value at iteration 1 is set to:

\[ {\bf x_{(1)}} = \left[ \begin{array}{c} z_{(1)}\\ (z_{(1)} - z_{(0)})/ \Delta t \end{array} \right] \]

The initial value $P_{(0|0)}$ of the prediction covariance matrix is given by:

\[ {\bf P_{(0|0)}} = \left[ \begin{array}{cc} \sigma^2_R & \frac{\sigma^2_R}{2 \Delta t}\\ \frac{\sigma^2_R}{2 \Delta t} & \frac{2}{3}\sigma^2_Q \Delta t + \frac{\sigma^2_R}{2 \Delta t^2} \end{array} \right] \]

Parameters
n_signal: Number of signal to filter.
sigma_state: Vector that fix the variance of the state covariance matrix $[\sigma^2_Q \; 0]^T$. The dimension of this vector is 2 multiplied by the number of signal to filter.
sigma_measure: Variance $\sigma^2_R$ of the measurement noise. The dimension of this vector is equal to the number of signal to filter.
delta_t: Sampling time $\Delta t$ expressed is second.

Definition at line 288 of file vpLinearKalmanFilterInstantiation.cpp.

References vpKalmanFilter::dt, vpKalmanFilter::F, vpKalmanFilter::H, vpKalmanFilter::init(), vpKalmanFilter::iter, vpKalmanFilter::Pest, vpKalmanFilter::Q, vpKalmanFilter::R, setStateModel(), vpKalmanFilter::size_measure, vpKalmanFilter::size_state, stateConstVel_MeasurePos, and vpKalmanFilter::Xest.

Referenced by initFilter().

◆ initStateConstVelWithColoredNoise_MeasureVel()

void vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel ( unsigned int  n_signal,
vpColVector &  sigma_state,
vpColVector &  sigma_measure,
double  rho 
)

Modelisation of a constant speed state model with colored noise. The measure is assumed to be the velocity of the target.

This state model assume that there is some memory associated with noise measurements as acceleration terms. They can be represented as remaining correlated (or colored) over successive time intervals, leading to the following state model:

\[ \left\{ \begin{array}{rll} x_{(k+1)} & = x_{(k)} + \nu_{(k)} &\\ \nu_{(k+1)}& = \rho \nu_{(k)} &+w_{(k)} \end{array} \right. \]

The term $w_{(k)}$ account for deviations from the assumed constant velocity trajectory. It is assumed zero-mean, white, mutually uncorrelated, stationary random variable with variance $\sigma^2_Q$. The term $\rho$ is the degree of correlation between successive accelerations. Values can range from 0 to 1.

We recall that the recursive state evolution equation is given by

\[ {\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\ \]

From this state model, the transition matrix ${\bf F}$ and the state covariance matrix ${\bf Q}$ are given by:

\[ {\bf F} = \left[ \begin{array}{cc} 1 & 1\\ 0 & \rho \end{array} \right] \]

and

\[ {\bf Q} = \left[ \begin{array}{cc} 0 & 0\\ 0 & \sigma^2_Q \end{array} \right] \]

The measurement model is given by:

\[ z_{(k)} = {\bf H} {\bf x}_{(k)} + r_{(k)} \]

where ${\bf H} = [1 \; 0 \; 0]$, $z_{(k)}$ is the measure of the velocity and $r_{(k)}$ is the measurement noise, assumed zero-mean, white mutually uncorrelated stationary random variables with variance $\sigma^2_R$, giving the covariance matrix:

\[ {\bf R} = \left[\sigma^2_R\right] \]

The initial value of the state vector is set to:

\[ {\bf x_{(0)}} = \left[ \begin{array}{c} z_{(0)}\\ 0 \end{array} \right] \]

The initial value $P_{(0|0)}$ of the prediction covariance matrix is given by:

\[ {\bf P_{(0|0)}} = \left[ \begin{array}{cc} \sigma^2_R & 0\\ 0 & \sigma^2_Q/(1-\rho^2) \end{array} \right] \]

Parameters
n_signal: Number of signal to filter.
sigma_state: Vector that fix the variance of the state covariance matrix $[0 \; \sigma^2_Q]^T$. The dimension of this vector is 2 multiplied by the number of signal to filter.
sigma_measure: Variance $\sigma^2_R$ of the measurement noise. The dimension of this vector is equal to the number of signal to filter.
rho: Degree of correlation between successive accelerations. Values are in [0:1[.
Exceptions
vpException::badValue: Bad rho value wich is not in [0:1[.

The example below shows how to filter a two dimensional target trajectory with an estimation of the target velocity from velocity measures.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
int main()
{
vpLinearKalmanFilterInstantiation kalman;
// Filter the x and y velocities of a target (2 signals are to consider)
int nsignal = 2;
// Initialize the filter parameters:
// - Firstly, the state variance
vpColVector sigma_state(4); // 4 = 2 for the state size x 2 signal
sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
// - Secondly, the measures variance
vpColVector sigma_measure(nsignal); // 2 velocity measures available
sigma_measure[0] = 0.03; // Variance on the x velocity measure
sigma_measure[1] = 0.06; // Variance on the y velocity measure
// - Thirdly, the correlation between successive accelerations
double rho = 0.9;
// Initialize the filter
kalman.initStateConstVelWithColoredNoise_MeasureVel(nsignal, sigma_state, sigma_measure, rho);
// Does the filtering
vpColVector vm(2); // Measured velocities
for ( ; ; ) {
// Get the two dimentional velocity measures
// vm[0] = ...;
// vm[1] = ...;
// Compute the filtering and the prediction
kalman.filter(vm);
// Print the estimation of the velocities (1st value of the state vector)
std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
<< std::endl;
// The one step prediction is available in kalman.Xpre variable
}
}

Definition at line 494 of file vpLinearKalmanFilterInstantiation.cpp.

References vpException::badValue, vpKalmanFilter::F, vpKalmanFilter::H, vpKalmanFilter::init(), vpKalmanFilter::iter, vpKalmanFilter::Pest, vpKalmanFilter::Q, vpKalmanFilter::R, setStateModel(), vpKalmanFilter::size_measure, vpKalmanFilter::size_state, stateConstVelWithColoredNoise_MeasureVel, vpERROR_TRACE, and vpKalmanFilter::Xest.

Referenced by initFilter().

◆ prediction()

void vpKalmanFilter::prediction ( )
inherited

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 vpKalmanFilter::F, vpException::fatalError, vpArray2D< Type >::getRows(), vpKalmanFilter::nsignal, vpKalmanFilter::Pest, vpKalmanFilter::Ppre, vpKalmanFilter::Q, vpKalmanFilter::size_state, vpMatrix::t(), vpKalmanFilter::verbose_mode, vpKalmanFilter::Xest, and vpKalmanFilter::Xpre.

Referenced by filter().

◆ setNumberOfSignal()

void vpKalmanFilter::setNumberOfSignal ( unsigned int  n_signal)
inlineinherited

Set the number of signal to filter.

Definition at line 138 of file vpKalmanFilter.h.

◆ setStateModel()

void vpLinearKalmanFilterInstantiation::setStateModel ( vpStateModel  mdl)
inline

Set the Kalman state model. Depending on the state model, we set the state vector size and the measure vector size.

The example below shows how to use this method and then to get the size of the state and measure vectors.

#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
int main()
{
vpLinearKalmanFilterInstantiation kalman;
kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);
// Value is 2
std::cout << "State vector size: " << kalman.getStateSize() << std::endl;
// Value is 1
std::cout << "Measure vector size: " << kalman.getMeasureSize() << std::endl;
}
Examples:
servoAfma4Point2DCamVelocityKalman.cpp, and servoViper850Point2DCamVelocityKalman.cpp.

Definition at line 147 of file vpLinearKalmanFilterInstantiation.h.

Referenced by initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), and initStateConstVelWithColoredNoise_MeasureVel().

◆ verbose()

void vpKalmanFilter::verbose ( bool  on)
inlineinherited

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
inherited

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

Definition at line 193 of file vpKalmanFilter.h.

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

◆ F

vpMatrix vpKalmanFilter::F
inherited

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

Definition at line 181 of file vpKalmanFilter.h.

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

◆ H

vpMatrix vpKalmanFilter::H
inherited

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

Definition at line 184 of file vpKalmanFilter.h.

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

◆ I

vpMatrix vpKalmanFilter::I
protectedinherited

Identity matrix $ \bf I$.

Definition at line 218 of file vpKalmanFilter.h.

Referenced by vpKalmanFilter::init().

◆ iter

long vpKalmanFilter::iter
protectedinherited

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

Definition at line 116 of file vpKalmanFilter.h.

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

◆ model

vpStateModel vpLinearKalmanFilterInstantiation::model
protected

Definition at line 122 of file vpLinearKalmanFilterInstantiation.h.

Referenced by filter(), and initFilter().

◆ nsignal

unsigned int vpKalmanFilter::nsignal
protectedinherited

Number of signal to filter.

Definition at line 123 of file vpKalmanFilter.h.

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

◆ Pest

vpMatrix vpKalmanFilter::Pest
inherited

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 vpKalmanFilter::filtering(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), and vpKalmanFilter::prediction().

◆ Ppre

vpMatrix vpKalmanFilter::Ppre
inherited

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 vpKalmanFilter::filtering(), and vpKalmanFilter::prediction().

◆ Q

vpMatrix vpKalmanFilter::Q
inherited

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

Definition at line 189 of file vpKalmanFilter.h.

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

◆ R

vpMatrix vpKalmanFilter::R
inherited

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

Definition at line 187 of file vpKalmanFilter.h.

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

◆ size_measure

unsigned int vpKalmanFilter::size_measure
protectedinherited

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

Definition at line 121 of file vpKalmanFilter.h.

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

◆ size_state

unsigned int vpKalmanFilter::size_state
protectedinherited

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

Definition at line 119 of file vpKalmanFilter.h.

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

◆ verbose_mode

bool vpKalmanFilter::verbose_mode
protectedinherited

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

Definition at line 127 of file vpKalmanFilter.h.

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

◆ W

vpMatrix vpKalmanFilter::W
protectedinherited

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 vpKalmanFilter::filtering().

◆ Xest

vpColVector vpKalmanFilter::Xest
inherited

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 filter(), vpKalmanFilter::filtering(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), and vpKalmanFilter::prediction().

◆ Xpre

vpColVector vpKalmanFilter::Xpre
inherited

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 vpKalmanFilter::filtering(), vpKalmanFilter::init(), and vpKalmanFilter::prediction().