Nonholonomic Motion model abstraction layer. More...
#include <motionModel.h>
Public Member Functions | |
void | initEstimator (Vector5d &x, Matrix5d &P) |
void | initEstimator (Vector3d &z) |
void | initFilter (Point measurement) |
void | makeA () |
Make the process Jacobian matrix. More... | |
void | makeBaseV () |
Make measurement noise sensitivity matrix. More... | |
void | makeBaseW () |
void | makeH () |
void | makeMeasure () |
Make measurement, used when measurement is not possible (i'm not using it now) More... | |
void | makeProcess () |
void | makeQ () |
Make process noise covariance matrix. More... | |
void | makeR () |
MotionModel () | |
MotionModel () | |
void | setIdentity (kMatrix &M, int size, double value=1) |
void | stepEstimator (Vector3d z) |
void | stepEstimator (Vector2d measurement) |
Public Attributes | |
nonholonomicEKFilter | _estimator |
Local estimator for process, Kalman filter with a constant velocity. More... | |
Matrix5d | _P |
Posteriori error covariance. More... | |
Vector5d | _x |
Estimated state. More... | |
Vector5d | _xp |
Predicted x. More... | |
Vector3d | _z |
Measurement vector. More... | |
double | dt |
Vector2d | inovation_error |
Matrix4d | PosteriorCovariance |
Vector4d | x_estimated |
Vector4d | x_predicted |
Nonholonomic Motion model abstraction layer.
This class encapsulates the underlaying motion model used. It provides an abstraction layer for the Mht class to work with, without having to deal with the Kalman filter directly.
Definition at line 46 of file motionModel.h.
MotionModel::MotionModel | ( | ) |
Definition at line 34 of file motionModel.cpp.
Definition at line 42 of file motionModel.cpp.
void MotionModel::initEstimator | ( | Vector3d & | z | ) |
Definition at line 48 of file motionModel.cpp.
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
void MotionModel::stepEstimator | ( | Vector3d | z | ) |
Definition at line 99 of file motionModel.cpp.
|
inline |
nonholonomicEKFilter MotionModel::_estimator |
Local estimator for process, Kalman filter with a constant velocity.
Definition at line 59 of file motionModel.h.
Matrix5d MotionModel::_P |
Posteriori error covariance.
Definition at line 56 of file motionModel.h.
Vector5d MotionModel::_x |
Estimated state.
Definition at line 50 of file motionModel.h.
Vector5d MotionModel::_xp |
Predicted x.
Definition at line 52 of file motionModel.h.
Vector3d MotionModel::_z |
Measurement vector.
Definition at line 54 of file motionModel.h.