Kalman filter with constant-velocity system model. The system noise is automatically calculated from the maximum expected acceleration which can be set as a parameter. More...
#include <KalmanFilter.h>
Public Member Functions | |
virtual KalmanFilter * | clone () const |
Clone operator. More... | |
const pbl::Gaussian & | getGaussian () const |
Returns the Kalman state and covariance as Gaussian. More... | |
double | getLikelihood (const pbl::Gaussian &z) const |
Calculates the likelihood that the given measurement originates from the estimated Kalman state. More... | |
const pbl::Vector & | getState () const |
Returns the Kalman state (mean of the estimated Gaussian) More... | |
const pbl::Matrix & | getStateCovariance () const |
Returns the covariance of the Kalman state. More... | |
virtual void | init (const pbl::Gaussian &G) |
Initializes the Kalman filter according to the given Gaussian, i.e., the initial state and state covariance is set to the mean and covariance of the given Gaussian. More... | |
KalmanFilter (int dim) | |
Constructs a Kalman filter with specified state dimensionality. More... | |
KalmanFilter (const KalmanFilter &orig) | |
Copy constructor. More... | |
virtual void | propagate (const double &dt) |
Propagates the state of the Kalman filter according to the (constant- velocity) system model. More... | |
void | setMaxAcceleration (double a_max) |
Set the maximum expected acceleration. This parameter is used to determine the system noise in the propagate phase: a constant velocity is assumed, but the system noise takes into account that the the target may accelerate or deccelerate with the specified maximum acceleration. More... | |
void | update (const pbl::Gaussian &z) |
Updates the Kalman filter with the given (Gaussian) measurement. More... | |
virtual | ~KalmanFilter () |
Destructor. More... | |
Protected Attributes | |
double | a_max_ |
pbl::Gaussian | G_ |
pbl::Gaussian | G_small_ |
pbl::Matrix | H_ |
int | meas_dim_ |
int | state_dim_ |
Kalman filter with constant-velocity system model. The system noise is automatically calculated from the maximum expected acceleration which can be set as a parameter.
Definition at line 46 of file KalmanFilter.h.
KalmanFilter::KalmanFilter | ( | int | dim | ) |
Constructs a Kalman filter with specified state dimensionality.
dim | Dimensionality of the state |
Definition at line 39 of file KalmanFilter.cpp.
KalmanFilter::KalmanFilter | ( | const KalmanFilter & | orig | ) |
Copy constructor.
Definition at line 43 of file KalmanFilter.cpp.
|
virtual |
Destructor.
Definition at line 47 of file KalmanFilter.cpp.
|
virtual |
Clone operator.
Definition at line 50 of file KalmanFilter.cpp.
const pbl::Gaussian & KalmanFilter::getGaussian | ( | ) | const |
Returns the Kalman state and covariance as Gaussian.
Definition at line 129 of file KalmanFilter.cpp.
double KalmanFilter::getLikelihood | ( | const pbl::Gaussian & | z | ) | const |
Calculates the likelihood that the given measurement originates from the estimated Kalman state.
z | The Gaussian measurement |
Definition at line 125 of file KalmanFilter.cpp.
const pbl::Vector & KalmanFilter::getState | ( | ) | const |
Returns the Kalman state (mean of the estimated Gaussian)
Definition at line 133 of file KalmanFilter.cpp.
const pbl::Matrix & KalmanFilter::getStateCovariance | ( | ) | const |
Returns the covariance of the Kalman state.
Definition at line 137 of file KalmanFilter.cpp.
|
virtual |
Initializes the Kalman filter according to the given Gaussian, i.e., the initial state and state covariance is set to the mean and covariance of the given Gaussian.
G | The Gaussian to initialize with |
Definition at line 54 of file KalmanFilter.cpp.
|
virtual |
Propagates the state of the Kalman filter according to the (constant- velocity) system model.
dt | The time increment with which to propagate |
Definition at line 64 of file KalmanFilter.cpp.
void KalmanFilter::setMaxAcceleration | ( | double | a_max | ) |
Set the maximum expected acceleration. This parameter is used to determine the system noise in the propagate phase: a constant velocity is assumed, but the system noise takes into account that the the target may accelerate or deccelerate with the specified maximum acceleration.
a_max | The maximum expected acceleration |
Definition at line 141 of file KalmanFilter.cpp.
void KalmanFilter::update | ( | const pbl::Gaussian & | z | ) |
Updates the Kalman filter with the given (Gaussian) measurement.
z | The Gaussian measurement |
Definition at line 102 of file KalmanFilter.cpp.
|
protected |
Maximum expected acceleration
Definition at line 144 of file KalmanFilter.h.
|
protected |
The estimated Kalman state as Gaussian (state mean and covariance)
Definition at line 135 of file KalmanFilter.h.
|
protected |
Portion of the estimated Kalman state that contains only the measurement dimensions
Definition at line 138 of file KalmanFilter.h.
|
protected |
Observation model
Definition at line 141 of file KalmanFilter.h.
|
protected |
Dimensionality of the measurements
Definition at line 129 of file KalmanFilter.h.
|
protected |
Dimensionality of the full state
Definition at line 132 of file KalmanFilter.h.