Extended Kalman Filter (EKF) implementation. More...
#include <Kalman.h>
Public Member Functions | |
KalmanEkf (int _n) | |
~KalmanEkf () | |
Public Member Functions inherited from alvar::Kalman | |
Kalman (int _n) | |
Constructor. More... | |
CvMat * | predict (unsigned long tick) |
Predict the Kalman state vector for the given time step This calls updateF for updating the transition matrix based on the real time step. More... | |
CvMat * | predict_update (KalmanSensor *sensor, unsigned long tick) |
Predict the Kalman state vector for the given time step and update the state using the Kalman gain. More... | |
double | seconds_since_update (unsigned long tick) |
Helper method. More... | |
~Kalman () | |
Destructor. More... | |
Public Member Functions inherited from alvar::KalmanCore | |
int | get_n () |
Accessor for n. More... | |
KalmanCore (const KalmanCore &s) | |
Copy constructor. More... | |
KalmanCore (int _n) | |
Constructor. More... | |
virtual CvMat * | predict () |
Predict the Kalman state vector for the given time step . x_pred = F * x. More... | |
CvMat * | predict_update (KalmanSensorCore *sensor) |
Predict the Kalman state vector and update the state using the constant Kalman gain. x = x_pred + K* ( z - H*x_pred) More... | |
~KalmanCore () | |
Destructor. More... | |
Protected Member Functions | |
virtual void | f (CvMat *_x, CvMat *_x_pred, double dt)=0 |
virtual void | predict_x (unsigned long tick) |
virtual void | update_F (unsigned long tick) |
Protected Member Functions inherited from alvar::Kalman | |
void | predict_P () |
Protected Attributes | |
CvMat * | delta |
CvMat * | x_minus |
CvMat * | x_plus |
CvMat * | x_tmp1 |
CvMat * | x_tmp2 |
Protected Attributes inherited from alvar::Kalman | |
int | prev_tick |
Protected Attributes inherited from alvar::KalmanCore | |
CvMat * | F_trans |
int | n |
Additional Inherited Members | |
Public Attributes inherited from alvar::Kalman | |
CvMat * | P |
The error covariance matrix describing the accuracy of the state estimate. More... | |
CvMat * | P_pred |
The predicted error covariance matrix. More... | |
CvMat * | Q |
The covariance matrix for the process noise. More... | |
Public Attributes inherited from alvar::KalmanCore | |
CvMat * | F |
The matrix (n*n) containing the transition model for the internal state. More... | |
CvMat * | x |
The Kalman state vector (n*1) More... | |
CvMat * | x_pred |
Predicted state, TODO: should be protected?! More... | |
Extended Kalman Filter (EKF) implementation.
Please override the pure virtual f() with the desired unlinear function. By default the upate_F calculates the Jacobian numerically, if you want other approach override also the update_F()
alvar::KalmanEkf::KalmanEkf | ( | int | _n | ) |
Definition at line 281 of file Kalman.cpp.
alvar::KalmanEkf::~KalmanEkf | ( | ) |
Definition at line 289 of file Kalman.cpp.
|
protectedpure virtual |
Implemented in KalmanOwn.
|
protectedvirtual |
Reimplemented from alvar::KalmanCore.
Definition at line 276 of file Kalman.cpp.
|
protectedvirtual |
If your transition matrix F is based on time you need to override this method.
Reimplemented from alvar::Kalman.
Definition at line 253 of file Kalman.cpp.