![]() |
Visual Servoing Platform version 3.6.0
|
#include <vpKalmanFilter.h>
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 |
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:
where
The measurement equation is given by:
where
The predicted state is obtained by:
where
Filtering equation are:
where
Notice that there is a recursion for the inverse covariance
where
ViSP provides different state evolution models implemented in the vpLinearKalmanFilterInstantiation class.
Definition at line 104 of file vpKalmanFilter.h.
vpKalmanFilter::vpKalmanFilter | ( | ) |
Construct a default Kalman filter.
The verbose mode is disabled by default
Definition at line 88 of file vpKalmanFilter.cpp.
|
explicit |
Construct a default Kalman filter by setting the number of signal to filter.
The verbose mode is disabled by default
n_signal | : Number of signal to filter. |
Definition at line 101 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 disabled by default
size_state_vector | : Size of the state vector ![]() |
size_measure_vector | : Size of the measure vector ![]() |
n_signal | : Number of signal to filter. |
Definition at line 120 of file vpKalmanFilter.cpp.
References init().
|
inlinevirtual |
Destructor that does noting.
Definition at line 128 of file vpKalmanFilter.h.
void vpKalmanFilter::filtering | ( | const vpColVector & | z | ) |
Update the Kalman filter by applying the filtering equations and increment the filter iteration (vpKalmanFilter::iter).
z | : Measure (or observation) ![]() ![]() |
The filtering equation is given by:
where
and where the updated covariance of the state is given by
or in a symmetric form
with
Definition at line 200 of file vpKalmanFilter.cpp.
References H, vpMatrix::inverseByLU(), iter, Pest, Ppre, R, vpMatrix::t(), verbose_mode, W, Xest, and Xpre.
Referenced by vpLinearKalmanFilterInstantiation::filter().
|
inline |
Return the iteration number.
Definition at line 153 of file vpKalmanFilter.h.
|
inline |
Return the size of the measure vector
Definition at line 145 of file vpKalmanFilter.h.
|
inline |
Return the number of signal to filter.
Definition at line 149 of file vpKalmanFilter.h.
|
inline |
Return the size of the state vector
Definition at line 141 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.
size_state_vector | : Size of the state vector ![]() |
size_measure_vector | : Size of the measure vector ![]() |
n_signal | : Number of signal to filter. |
Definition at line 57 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
and the state prediction covariance by
Definition at line 143 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().
|
inline |
Set the number of signal to filter.
Definition at line 132 of file vpKalmanFilter.h.
|
inline |
Sets the verbose mode.
on | : If true, activates the verbose mode which consists in printing the Kalman filter internal values. |
Definition at line 159 of file vpKalmanFilter.h.
double vpKalmanFilter::dt |
Sampling time
Definition at line 187 of file vpKalmanFilter.h.
Referenced by vpLinearKalmanFilterInstantiation::filter(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), and vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos().
vpMatrix vpKalmanFilter::F |
Transition matrix
Definition at line 175 of file vpKalmanFilter.h.
Referenced by init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and prediction().
vpMatrix vpKalmanFilter::H |
Matrix
Definition at line 178 of file vpKalmanFilter.h.
Referenced by filtering(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), and vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel().
|
protected |
|
protected |
Filter step or iteration. When set to zero, initialize the filter.
Definition at line 110 of file vpKalmanFilter.h.
Referenced by vpLinearKalmanFilterInstantiation::filter(), filtering(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), and vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel().
|
protected |
Number of signal to filter.
Definition at line 117 of file vpKalmanFilter.h.
Referenced by vpLinearKalmanFilterInstantiation::filter(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), and prediction().
vpMatrix vpKalmanFilter::Pest |
The updated covariance of the state
Definition at line 201 of file vpKalmanFilter.h.
Referenced by filtering(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and prediction().
vpMatrix vpKalmanFilter::Ppre |
The state prediction covariance
Definition at line 194 of file vpKalmanFilter.h.
Referenced by filtering(), and prediction().
vpMatrix vpKalmanFilter::Q |
Process noise covariance matrix
Definition at line 183 of file vpKalmanFilter.h.
Referenced by init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and prediction().
vpMatrix vpKalmanFilter::R |
Measurement noise covariance matrix
Definition at line 181 of file vpKalmanFilter.h.
Referenced by filtering(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), and vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel().
|
protected |
Size of the measure vector
Definition at line 115 of file vpKalmanFilter.h.
Referenced by vpLinearKalmanFilterInstantiation::filter(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and vpLinearKalmanFilterInstantiation::setStateModel().
|
protected |
Size of the state vector
Definition at line 113 of file vpKalmanFilter.h.
Referenced by vpLinearKalmanFilterInstantiation::filter(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), prediction(), and vpLinearKalmanFilterInstantiation::setStateModel().
|
protected |
When set to true, print the content of internal variables during filtering() and prediction().
Definition at line 121 of file vpKalmanFilter.h.
Referenced by filtering(), and prediction().
|
protected |
vpColVector vpKalmanFilter::Xest |
The updated state estimate
Definition at line 167 of file vpKalmanFilter.h.
Referenced by vpLinearKalmanFilterInstantiation::filter(), filtering(), init(), vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(), vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(), vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(), and prediction().
vpColVector vpKalmanFilter::Xpre |
The predicted state
Definition at line 172 of file vpKalmanFilter.h.
Referenced by filtering(), init(), and prediction().