#include <mrpt/bayes/CKalmanFilterCapable.h>
Classes | |
struct | TKF_options |
Generic options for the Kalman Filter algorithm in itself. More... | |
Public Member Functions | |
CKalmanFilterCapable () | |
Default constructor. | |
virtual | ~CKalmanFilterCapable () |
Destructor. | |
Public Attributes | |
TKF_options | KF_options |
Generic options for the Kalman Filter algorithm in itself. | |
Protected Member Functions | |
void | runOneKalmanIteration () |
The main entry point, executes one complete step: prediction + update. | |
Virtual methods for Kalman Filter implementation | |
virtual void | OnGetAction (CVectorTemplate< KFTYPE > &out_u)=0 |
Must return the action vector u. | |
virtual void | OnTransitionModel (const CVectorTemplate< KFTYPE > &in_u, CVectorTemplate< KFTYPE > &inout_x, bool &out_skipPrediction)=0 |
Implements the transition model ![]() | |
virtual void | OnTransitionJacobian (CMatrixTemplateNumeric< KFTYPE > &out_F)=0 |
Implements the transition Jacobian ![]() | |
virtual void | OnTransitionNoise (CMatrixTemplateNumeric< KFTYPE > &out_Q)=0 |
Implements the transition noise covariance ![]() | |
virtual void | OnGetObservations (CMatrixTemplateNumeric< KFTYPE > &out_z, CVectorTemplate< KFTYPE > &out_R2, vector_int &out_data_association) |
This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable. | |
virtual void | OnGetObservations (CMatrixTemplateNumeric< KFTYPE > &out_z, CMatrixTemplateNumeric< KFTYPE > &out_R, vector_int &out_data_association) |
This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable. | |
virtual void | OnObservationModelAndJacobians (const CMatrixTemplateNumeric< KFTYPE > &in_z, const vector_int &in_data_association, const bool &in_full, const int &in_obsIdx, CVectorTemplate< KFTYPE > &out_innov, CMatrixTemplateNumeric< KFTYPE > &out_Hx, CMatrixTemplateNumeric< KFTYPE > &out_Hy)=0 |
Implements the observation prediction ![]() ![]() ![]() | |
virtual void | OnInverseObservationModel (const CMatrixTemplateNumeric< KFTYPE > &in_z, const size_t &in_obsIdx, const size_t &in_idxNewFeat, CVectorTemplate< KFTYPE > &out_yn, CMatrixTemplateNumeric< KFTYPE > &out_dyn_dxv, CMatrixTemplateNumeric< KFTYPE > &out_dyn_dhn) |
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". | |
virtual void | OnNormalizeStateVector () |
This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it. | |
virtual void | OnPostIteration () |
This method is called after finishing one KF iteration and before returning from runOneKalmanIteration(). | |
virtual size_t | get_vehicle_size () const =0 |
Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable. | |
virtual size_t | get_feature_size () const |
Must return the dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented). | |
virtual size_t | get_observation_size () const =0 |
Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc). | |
virtual size_t | get_vehicle_state_offset () const |
The index of the first component of the vehicle state within the whole state vector/matrix (default=0). | |
virtual size_t | get_feature_state_offset () const |
The index of the first component of the features part in the state vector/matrix (default=after the vehicle state, assuming it is at the begining of the vector). | |
Protected Attributes | |
math::CVectorTemplate< KFTYPE > | m_xkk |
The system state vector. | |
math::CMatrixTemplateNumeric < KFTYPE > | m_pkk |
The system full covariance matrix. | |
math::CMatrixTemplateNumeric < KFTYPE > | m_S |
The kalman innovation matrix using in some applications (i.e. |
This base class stores the state vector and covariance matrix of the system. It has virtual methods that must be completed by derived classes to address a given filtering problem. The main entry point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which should be called AFTER setting the desired filter options in KF_options, as well as any options in the derived class. Note that the main entry point is protected, so derived classes must offer another method more specific to a given problem which, internally, calls runOneKalmanIteration.
For further details and examples, check out the tutorial: http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters
Definition at line 74 of file CKalmanFilterCapable.h.
mrpt::bayes::CKalmanFilterCapable::CKalmanFilterCapable | ( | ) |
Default constructor.
virtual mrpt::bayes::CKalmanFilterCapable::~CKalmanFilterCapable | ( | ) | [virtual] |
Destructor.
virtual size_t mrpt::bayes::CKalmanFilterCapable::get_feature_size | ( | ) | const [inline, protected, virtual] |
Must return the dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
Definition at line 223 of file CKalmanFilterCapable.h.
virtual size_t mrpt::bayes::CKalmanFilterCapable::get_feature_state_offset | ( | ) | const [inline, protected, virtual] |
The index of the first component of the features part in the state vector/matrix (default=after the vehicle state, assuming it is at the begining of the vector).
Definition at line 241 of file CKalmanFilterCapable.h.
virtual size_t mrpt::bayes::CKalmanFilterCapable::get_observation_size | ( | ) | const [protected, pure virtual] |
Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
Implemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
virtual size_t mrpt::bayes::CKalmanFilterCapable::get_vehicle_size | ( | ) | const [protected, pure virtual] |
Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
Implemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
virtual size_t mrpt::bayes::CKalmanFilterCapable::get_vehicle_state_offset | ( | ) | const [inline, protected, virtual] |
The index of the first component of the vehicle state within the whole state vector/matrix (default=0).
Definition at line 234 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable::OnGetAction | ( | CVectorTemplate< KFTYPE > & | out_u | ) | [protected, pure virtual] |
Must return the action vector u.
out_u | The action vector which will be passed to OnTransitionModel |
Implemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
virtual void mrpt::bayes::CKalmanFilterCapable::OnGetObservations | ( | CMatrixTemplateNumeric< KFTYPE > & | out_z, | |
CMatrixTemplateNumeric< KFTYPE > & | out_R, | |||
vector_int & | out_data_association | |||
) | [protected, virtual] |
This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.
out_z | A ![]() | |
out_R | A matrix of size N·OxO (O being get_observation_size() and N the number of observations). It is the covariance matrix of the sensor noise for each of the returned observations. The order must correspond to that in out_z. | |
out_data_association | An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. This method will be called just once for each complete KF iteration. |
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
virtual void mrpt::bayes::CKalmanFilterCapable::OnGetObservations | ( | CMatrixTemplateNumeric< KFTYPE > & | out_z, | |
CVectorTemplate< KFTYPE > & | out_R2, | |||
vector_int & | out_data_association | |||
) | [inline, protected, virtual] |
This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.
out_z | A ![]() | |
out_R2 | A vector of size O (O being get_observation_size()) with the *variances* of the sensor noise for each of the observation components. This is constant for all the observations (where N>1), and in the naive Kalman method the value TKF_options::veryLargeR2 is used for unobserved map elements. | |
out_data_association | An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. This method will be called just once for each complete KF iteration. |
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 139 of file CKalmanFilterCapable.h.
References MRPT_TRY_END, MRPT_TRY_START, and THROW_EXCEPTION.
Referenced by mrpt::slam::CRangeBearingKFSLAM2D::OnGetObservations(), and mrpt::slam::CRangeBearingKFSLAM::OnGetObservations().
virtual void mrpt::bayes::CKalmanFilterCapable::OnInverseObservationModel | ( | const CMatrixTemplateNumeric< KFTYPE > & | in_z, | |
const size_t & | in_obsIdx, | |||
const size_t & | in_idxNewFeat, | |||
CVectorTemplate< KFTYPE > & | out_yn, | |||
CMatrixTemplateNumeric< KFTYPE > & | out_dyn_dxv, | |||
CMatrixTemplateNumeric< KFTYPE > & | out_dyn_dhn | |||
) | [protected, virtual] |
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
in_z | This is the same matrix returned by OnGetObservations(). | |
in_obsIndex | The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found. | |
in_idxNewFeat | The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices. | |
out_yn | The F-length vector with the inverse observation model ![]() | |
out_dyn_dxv | The ![]() ![]() | |
out_dyn_dhn | The ![]() ![]() |
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
virtual void mrpt::bayes::CKalmanFilterCapable::OnNormalizeStateVector | ( | ) | [protected, virtual] |
This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
virtual void mrpt::bayes::CKalmanFilterCapable::OnObservationModelAndJacobians | ( | const CMatrixTemplateNumeric< KFTYPE > & | in_z, | |
const vector_int & | in_data_association, | |||
const bool & | in_full, | |||
const int & | in_obsIdx, | |||
CVectorTemplate< KFTYPE > & | out_innov, | |||
CMatrixTemplateNumeric< KFTYPE > & | out_Hx, | |||
CMatrixTemplateNumeric< KFTYPE > & | out_Hy | |||
) | [protected, pure virtual] |
Implements the observation prediction and the Jacobians
and (when applicable)
.
in_z | This is the same matrix returned by OnGetObservations(), passed here for reference. | |
in_data_association | The vector returned by OnGetObservations(), passed here for reference. | |
in_full | If set to true, all the Jacobians and predictions must be computed at once. Otherwise, just those for the observation in_obsIdx. | |
in_obsIdx | If in_full=false, the row of the observation (in in_z and in_data_association) whose innovation & Jacobians are to be returned now. | |
out_innov | The difference between the expected observation and the real one: ![]() | |
out_Hx | One or a vertical stack of ![]() | |
out_Hy | An empty matrix, or one or a vertical stack of ![]() |
Implemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
virtual void mrpt::bayes::CKalmanFilterCapable::OnPostIteration | ( | ) | [protected, virtual] |
This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
Reimplemented in mrpt::monoslam::CMonoSlam.
virtual void mrpt::bayes::CKalmanFilterCapable::OnTransitionJacobian | ( | CMatrixTemplateNumeric< KFTYPE > & | out_F | ) | [protected, pure virtual] |
Implements the transition Jacobian .
out_F | Must return the Jacobian. The returned matrix must be ![]() |
Implemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
virtual void mrpt::bayes::CKalmanFilterCapable::OnTransitionModel | ( | const CVectorTemplate< KFTYPE > & | in_u, | |
CVectorTemplate< KFTYPE > & | inout_x, | |||
bool & | out_skipPrediction | |||
) | [protected, pure virtual] |
Implements the transition model .
in_u | The vector returned by OnGetAction. | |
inout_x | At input has
, at output must have | |
out_skip | Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false |
Implemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
virtual void mrpt::bayes::CKalmanFilterCapable::OnTransitionNoise | ( | CMatrixTemplateNumeric< KFTYPE > & | out_Q | ) | [protected, pure virtual] |
Implements the transition noise covariance .
out_Q | Must return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian |
Implemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and mrpt::monoslam::CMonoSlam.
void mrpt::bayes::CKalmanFilterCapable::runOneKalmanIteration | ( | ) | [protected] |
The main entry point, executes one complete step: prediction + update.
It is protected since derived classes must provide a problem-specific entry point for users. The exact order in which this method calls the virtual method is explained in http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters.
Generic options for the Kalman Filter algorithm in itself.
Definition at line 294 of file CKalmanFilterCapable.h.
The kalman innovation matrix using in some applications (i.e.
MonoSLAM).
Definition at line 87 of file CKalmanFilterCapable.h.
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 22:58:25 EDT 2009 |