![]() |
Visual Servoing Platform
version 3.2.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 is the unknown state at iteration
.
The measurement equation is given by:
where is the measure (also named observation) at iteration
.
The predicted state is obtained by:
where
Filtering equation are:
where is the filter gain.
Notice that there is a recursion for the inverse covariance
where is the inverse of the covariance matrix.
ViSP provides different state evolution models implemented in the vpLinearKalmanFilterInstantiation class.
Definition at line 109 of file vpKalmanFilter.h.
vpKalmanFilter::vpKalmanFilter | ( | ) |
Construct a default Kalman filter.
The verbose mode is by default desactivated.
Definition at line 91 of file vpKalmanFilter.cpp.
|
explicit |
Construct a default Kalman filter by setting the number of signal to filter.
The verbose mode is by default desactivated.
n_signal | : Number of signal to filter. |
Definition at line 104 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.
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 123 of file vpKalmanFilter.cpp.
|
inlinevirtual |
Destructor that does noting.
Definition at line 133 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 is the filter gain computed using the formula:
and where the updated covariance of the state is given by
or in a symetric form
with
Definition at line 210 of file vpKalmanFilter.cpp.
|
inline |
Return the iteration number.
Definition at line 158 of file vpKalmanFilter.h.
|
inline |
Return the size of the measure vector for one signal.
Definition at line 150 of file vpKalmanFilter.h.
|
inline |
Return the number of signal to filter.
Definition at line 154 of file vpKalmanFilter.h.
|
inline |
Return the size of the state vector for one signal.
Definition at line 146 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 60 of file vpKalmanFilter.cpp.
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 146 of file vpKalmanFilter.cpp.
|
inline |
Set the number of signal to filter.
Definition at line 137 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 164 of file vpKalmanFilter.h.
double vpKalmanFilter::dt |
Sampling time in second between two succesive iterations. Only used in some specific state models implemented in vpLinearKalmanFilterInstantiation.
Definition at line 192 of file vpKalmanFilter.h.
vpMatrix vpKalmanFilter::F |
Transition matrix that describes the evolution of the state.
Definition at line 180 of file vpKalmanFilter.h.
vpMatrix vpKalmanFilter::H |
Matrix that describes the evolution of the measurements.
Definition at line 183 of file vpKalmanFilter.h.
|
protected |
Identity matrix .
Definition at line 217 of file vpKalmanFilter.h.
|
protected |
Filter step or iteration. When set to zero, initialize the filter.
Definition at line 115 of file vpKalmanFilter.h.
|
protected |
Number of signal to filter.
Definition at line 122 of file vpKalmanFilter.h.
vpMatrix vpKalmanFilter::Pest |
The updated covariance of the state where
.
Definition at line 206 of file vpKalmanFilter.h.
vpMatrix vpKalmanFilter::Ppre |
The state prediction covariance where
.
Definition at line 199 of file vpKalmanFilter.h.
vpMatrix vpKalmanFilter::Q |
Process noise covariance matrix .
Definition at line 188 of file vpKalmanFilter.h.
vpMatrix vpKalmanFilter::R |
Measurement noise covariance matrix .
Definition at line 186 of file vpKalmanFilter.h.
|
protected |
Size of the measure vector .
Definition at line 120 of file vpKalmanFilter.h.
|
protected |
Size of the state vector .
Definition at line 118 of file vpKalmanFilter.h.
|
protected |
When set to true, print the content of internal variables during filtering() and prediction().
Definition at line 126 of file vpKalmanFilter.h.
|
protected |
Filter gain where
.
Definition at line 214 of file vpKalmanFilter.h.
vpColVector vpKalmanFilter::Xest |
The updated state estimate where
.
Definition at line 164 of file vpKalmanFilter.h.
vpColVector vpKalmanFilter::Xpre |
The predicted state where
.
Definition at line 177 of file vpKalmanFilter.h.