Visual Servoing Platform
version 3.6.1 under development (2024-10-15)
|
#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
Public Types | |
enum | vpStateModel { stateConstVel_MeasurePos , stateConstVelWithColoredNoise_MeasureVel , stateConstAccWithColoredNoise_MeasureVel , unknown } |
Public Member Functions | |
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) |
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 |
Linear filter initializer with constant acceleration models | |
vpStateModel | model |
void | initStateConstAccWithColoredNoise_MeasureVel (unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt) |
This class provides an implementation of some specific linear Kalman filters.
Definition at line 53 of file vpLinearKalmanFilterInstantiation.h.
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 59 of file vpLinearKalmanFilterInstantiation.h.
|
inline |
Default linear Kalman filter.
By default the state model is unknown and set to vpLinearKalmanFilterInstantiation::unknown.
Definition at line 86 of file vpLinearKalmanFilterInstantiation.h.
void vpLinearKalmanFilterInstantiation::filter | ( | vpColVector & | z | ) |
Do the filtering and prediction of the measure signal.
z | : Measures 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()) . |
vpException::notInitialized | : If the filter is not initialized. To initialize the filter see initFilter(). |
Definition at line 803 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, and vpKalmanFilter::Xest.
|
inherited |
Update the Kalman filter by applying the filtering equations and increment the filter iteration (vpKalmanFilter::iter).
z | : Measure (or observation) provided at iteration . |
The filtering equation is given by:
where is the filter gain computed using the formula:
and where the updated covariance of the state is given by
or in a symmetric form
with
Definition at line 199 of file vpKalmanFilter.cpp.
References vpKalmanFilter::H, vpMatrix::inverseByLU(), vpKalmanFilter::iter, vpKalmanFilter::Pest, vpKalmanFilter::Ppre, vpKalmanFilter::R, vpMatrix::t(), vpKalmanFilter::verbose_mode, vpKalmanFilter::W, vpKalmanFilter::Xest, and vpKalmanFilter::Xpre.
Referenced by filter().
|
inlineinherited |
Return the iteration number.
Definition at line 154 of file vpKalmanFilter.h.
|
inlineinherited |
Return the size of the measure vector for one signal.
Definition at line 146 of file vpKalmanFilter.h.
|
inlineinherited |
Return the number of signal to filter.
Definition at line 150 of file vpKalmanFilter.h.
|
inline |
Return the current state model.
Definition at line 91 of file vpLinearKalmanFilterInstantiation.h.
|
inlineinherited |
Return the size of the state vector for one signal.
Definition at line 142 of file vpKalmanFilter.h.
|
inherited |
Initialize the Kalman filter.
size_state_vector | : Size of the state vector for one signal. |
size_measure_vector | : Size of the measure vector for one signal. |
n_signal | : Number of signal to filter. |
Definition at line 58 of file vpKalmanFilter.cpp.
References vpKalmanFilter::dt, vpKalmanFilter::F, vpKalmanFilter::H, vpKalmanFilter::I, vpKalmanFilter::iter, vpKalmanFilter::nsignal, vpKalmanFilter::Pest, vpKalmanFilter::Q, vpKalmanFilter::R, vpColVector::resize(), vpArray2D< Type >::resize(), vpKalmanFilter::size_measure, vpKalmanFilter::size_state, vpKalmanFilter::Xest, and vpKalmanFilter::Xpre.
Referenced by initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), and vpKalmanFilter::vpKalmanFilter().
BEGIN_VISP_NAMESPACE 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().
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 size multiplied by the number of signal to filter. Values are used to initialize the 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 size multiplied by the number of signal to filter. Values are used to initialize the measure covariance matrix. |
rho | : Degree of correlation between successive accelerations. Values are in [0:1[. |
delta_t | : Sampling time 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(). |
vpException::badValue | : Bad rho value which 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.
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.
Definition at line 179 of file vpLinearKalmanFilterInstantiation.cpp.
References initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), model, vpException::notInitialized, stateConstAccWithColoredNoise_MeasureVel, stateConstVel_MeasurePos, stateConstVelWithColoredNoise_MeasureVel, and unknown.
void vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel | ( | unsigned int | n_signal, |
vpColVector & | sigma_state, | ||
vpColVector & | sigma_measure, | ||
double | rho, | ||
double | delta_t | ||
) |
Modelization 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:
The terms and account for deviations from the assumed constant acceleration trajectory. They are assumed zero-mean, white, mutually uncorrelated, stationary random variable with variance and . The term 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
From this state model, the transition matrix and the state covariance matrix are given by:
and
The measurement model is given by:
where , is the measure of the velocity and is the measurement noise, assumed zero-mean, white mutually uncorrelated stationary random variables with variance , giving the covariance matrix:
The initial value of the state vector is set to:
The initial value of the prediction covariance matrix is given by:
n_signal | : Number of signal to filter. |
sigma_state | : Vector that fix the variance of the state covariance matrix . The dimension of this vector is 3 multiplied by the number of signal to filter. |
sigma_measure | : Variance 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 expressed is second. |
vpException::badValue | : Bad rho value which 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.
Definition at line 727 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, and vpKalmanFilter::Xest.
Referenced by initFilter().
void vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos | ( | unsigned int | n_signal, |
vpColVector & | sigma_state, | ||
vpColVector & | sigma_measure, | ||
double | delta_t | ||
) |
Modelization 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
The terms and account for deviations from the assumed constant velocity trajectory. They are assumed zero-mean, white, mutually uncorrelated, stationary random variable with variance and .
We recall that the recursive state evolution equation is given by
From this state model, the transition matrix and the state covariance matrix are given by:
and
The initial value of the state vector at iteration 0 is set to:
The value at iteration 1 is set to:
The initial value of the prediction covariance matrix is given by:
n_signal | : Number of signal to filter. |
sigma_state | : Vector that fix the variance of the state covariance matrix . The dimension of this vector is 2 multiplied by the number of signal to filter. |
sigma_measure | : Variance of the measurement noise. The dimension of this vector is equal to the number of signal to filter. |
delta_t | : Sampling time expressed is second. |
Definition at line 293 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().
void vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel | ( | unsigned int | n_signal, |
vpColVector & | sigma_state, | ||
vpColVector & | sigma_measure, | ||
double | rho | ||
) |
Modelization 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:
The term account for deviations from the assumed constant velocity trajectory. It is assumed zero-mean, white, mutually uncorrelated, stationary random variable with variance . The term 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
From this state model, the transition matrix and the state covariance matrix are given by:
and
The measurement model is given by:
where , is the measure of the velocity and is the measurement noise, assumed zero-mean, white mutually uncorrelated stationary random variables with variance , giving the covariance matrix:
The initial value of the state vector is set to:
The initial value of the prediction covariance matrix is given by:
n_signal | : Number of signal to filter. |
sigma_state | : Vector that fix the variance of the state covariance matrix . The dimension of this vector is 2 multiplied by the number of signal to filter. |
sigma_measure | : Variance 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[. |
vpException::badValue | : Bad rho value which 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.
Definition at line 503 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, and vpKalmanFilter::Xest.
Referenced by initFilter().
|
inherited |
Update the Kalman filter by applying the prediction equations.
The predicted state is given by
and the state prediction covariance by
Definition at line 142 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().
|
inlineinherited |
Set the number of signal to filter.
Definition at line 133 of file vpKalmanFilter.h.
|
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.
Definition at line 145 of file vpLinearKalmanFilterInstantiation.h.
References model, vpKalmanFilter::size_measure, vpKalmanFilter::size_state, stateConstAccWithColoredNoise_MeasureVel, stateConstVel_MeasurePos, stateConstVelWithColoredNoise_MeasureVel, and unknown.
Referenced by initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), and initStateConstVelWithColoredNoise_MeasureVel().
|
inlineinherited |
Sets the verbose mode.
on | : If true, activates the verbose mode which consists in printing the Kalman filter internal values. |
Definition at line 160 of file vpKalmanFilter.h.
|
inherited |
Sampling time 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 filter(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), and initStateConstVel_MeasurePos().
|
inherited |
Transition matrix that describes the evolution of the state.
Definition at line 176 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), and vpKalmanFilter::prediction().
|
inherited |
Matrix that describes the evolution of the measurements.
Definition at line 179 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::filtering(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), and initStateConstVelWithColoredNoise_MeasureVel().
|
protectedinherited |
Identity matrix .
Definition at line 213 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::init().
|
protectedinherited |
Filter step or iteration. When set to zero, initialize the filter.
Definition at line 111 of file vpKalmanFilter.h.
Referenced by filter(), vpKalmanFilter::filtering(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), and initStateConstVelWithColoredNoise_MeasureVel().
|
protected |
Definition at line 116 of file vpLinearKalmanFilterInstantiation.h.
Referenced by filter(), initFilter(), and setStateModel().
|
protectedinherited |
Number of signal to filter.
Definition at line 118 of file vpKalmanFilter.h.
Referenced by filter(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), and vpKalmanFilter::prediction().
|
inherited |
The updated covariance of the state where .
Definition at line 202 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::filtering(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), and vpKalmanFilter::prediction().
|
inherited |
The state prediction covariance where .
Definition at line 195 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::filtering(), and vpKalmanFilter::prediction().
|
inherited |
Process noise covariance matrix .
Definition at line 184 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), and vpKalmanFilter::prediction().
|
inherited |
Measurement noise covariance matrix .
Definition at line 182 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::filtering(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), and initStateConstVelWithColoredNoise_MeasureVel().
|
protectedinherited |
Size of the measure vector .
Definition at line 116 of file vpKalmanFilter.h.
Referenced by filter(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), and setStateModel().
|
protectedinherited |
Size of the state vector .
Definition at line 114 of file vpKalmanFilter.h.
Referenced by filter(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), vpKalmanFilter::prediction(), and setStateModel().
|
protectedinherited |
When set to true, print the content of internal variables during filtering() and prediction().
Definition at line 122 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::filtering(), and vpKalmanFilter::prediction().
|
protectedinherited |
Filter gain where .
Definition at line 210 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::filtering().
|
inherited |
The updated state estimate where .
Definition at line 168 of file vpKalmanFilter.h.
Referenced by filter(), vpKalmanFilter::filtering(), vpKalmanFilter::init(), initStateConstAccWithColoredNoise_MeasureVel(), initStateConstVel_MeasurePos(), initStateConstVelWithColoredNoise_MeasureVel(), and vpKalmanFilter::prediction().
|
inherited |
The predicted state where .
Definition at line 173 of file vpKalmanFilter.h.
Referenced by vpKalmanFilter::filtering(), vpKalmanFilter::init(), and vpKalmanFilter::prediction().