template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
class mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
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://www.mrpt.org/Kalman_Filters
The Kalman filter algorithms are generic, but this implementation is biased to ease the implementation of SLAM-like problems. However, it can be also applied to many generic problems not related to robotics or SLAM.
The meaning of the template parameters is:
- VEH_SIZE: The dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
- OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
- FEAT_SIZE: The dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
- ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).
- KFTYPE: The numeric type of the matrices (default: double)
Revisions:
- 2007: Antonio J. Ortiz de Galisteo (AJOGD)
- 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
- 2008/MAR: Implemented IKF (JLBC).
- 2009/DEC: Totally rewritten as a generic template using fixed-size matrices where possible (JLBC).
- See also
- mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D
Definition at line 52 of file CKalmanFilterCapable.h.
|
using | kftype = KFTYPE |
| The numeric type used in the Kalman Filter (default=double) More...
|
|
using | KFCLASS = CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > |
| My class, in a shorter name! More...
|
|
using | KFVector = Eigen::Matrix< KFTYPE, Eigen::Dynamic, 1 > |
|
using | KFMatrix = mrpt::math::CMatrixTemplateNumeric< KFTYPE > |
|
using | KFMatrix_VxV = mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, VEH_SIZE > |
|
using | KFMatrix_OxO = mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, OBS_SIZE > |
|
using | KFMatrix_FxF = mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, FEAT_SIZE > |
|
using | KFMatrix_AxA = mrpt::math::CMatrixFixedNumeric< KFTYPE, ACT_SIZE, ACT_SIZE > |
|
using | KFMatrix_VxO = mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, OBS_SIZE > |
|
using | KFMatrix_VxF = mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, FEAT_SIZE > |
|
using | KFMatrix_FxV = mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, VEH_SIZE > |
|
using | KFMatrix_FxO = mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, OBS_SIZE > |
|
using | KFMatrix_OxF = mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, FEAT_SIZE > |
|
using | KFMatrix_OxV = mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, VEH_SIZE > |
|
using | KFArray_VEH = mrpt::math::CArrayNumeric< KFTYPE, VEH_SIZE > |
|
using | KFArray_ACT = mrpt::math::CArrayNumeric< KFTYPE, ACT_SIZE > |
|
using | KFArray_OBS = mrpt::math::CArrayNumeric< KFTYPE, OBS_SIZE > |
|
using | vector_KFArray_OBS = mrpt::aligned_std_vector< KFArray_OBS > |
|
using | KFArray_FEAT = mrpt::math::CArrayNumeric< KFTYPE, FEAT_SIZE > |
|
|
virtual void | OnInverseObservationModel (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const |
| If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
|
|
virtual void | OnInverseObservationModel (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn, KFMatrix_FxF &out_dyn_dhn_R_dyn_dhnT, bool &out_use_dyn_dhn_jacobian) const |
| If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
|
|
virtual void | OnNewLandmarkAddedToMap (const size_t in_obsIdx, const size_t in_idxNewFeat) |
| If applicable to the given problem, do here any special handling of adding a new landmark to the map. More...
|
|
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. More...
|
|
virtual void | OnPostIteration () |
| This method is called after finishing one KF iteration and before returning from runOneKalmanIteration(). More...
|
|
virtual void | OnGetAction (KFArray_ACT &out_u) const =0 |
| Must return the action vector u. More...
|
|
virtual void | OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const =0 |
| Implements the transition model . More...
|
|
virtual void | OnTransitionJacobian (KFMatrix_VxV &out_F) const |
| Implements the transition Jacobian . More...
|
|
virtual void | OnTransitionJacobianNumericGetIncrements (KFArray_VEH &out_increments) const |
| Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
|
|
virtual void | OnTransitionNoise (KFMatrix_VxV &out_Q) const =0 |
| Implements the transition noise covariance . More...
|
|
virtual void | OnPreComputingPredictions (const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const |
| This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made. More...
|
|
virtual void | OnGetObservationNoise (KFMatrix_OxO &out_R) const =0 |
| Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. More...
|
|
virtual void | OnGetObservationsAndDataAssociation (vector_KFArray_OBS &out_z, std::vector< int > &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::vector< size_t > &in_lm_indices_in_S, const KFMatrix_OxO &in_R)=0 |
| This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map. More...
|
|
virtual void | OnObservationModel (const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const =0 |
| Implements the observation prediction . More...
|
|
virtual void | OnObservationJacobians (const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const |
| Implements the observation Jacobians and (when applicable) . More...
|
|
virtual void | OnObservationJacobiansNumericGetIncrements (KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const |
| Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
|
|
virtual void | OnSubstractObservationVectors (KFArray_OBS &A, const KFArray_OBS &B) const |
| Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles). More...
|
|
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
- Parameters
-
out_R | The noise covariance matrix. It might be non diagonal, but it'll usually be. |
- Note
- Upon call, it can be assumed that the previous contents of out_R are all zeros.
Implemented in mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D, and CRangeBearing.
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map.
- Parameters
-
out_z | N vectors, each for one "observation" of length OBS_SIZE, N being the number of "observations": how many observed landmarks for a map, or just one if not applicable. |
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 (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration. |
in_all_predictions | A vector with the prediction of ALL the landmarks in the map. Note that, in contrast, in_S only comprises a subset of all the landmarks. |
in_S | The full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a M*O x M*O matrix with M=length of "in_lm_indices_in_S". |
in_lm_indices_in_S | The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S. |
This method will be called just once for each complete KF iteration.
- Note
- It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian), and another from the uncertainty in the observation itself. By default, out_use_dyn_dhn_jacobian=true on call, and if it's left at "true", the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn. Only in some problems (e.g. MonoSLAM), it'll be needed for the application to directly return the covariance matrix out_dyn_dhn_R_dyn_dhnT, which is the equivalent to:
\f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial
y_n}{\partial h_n}^\top y_n=y(x,z_n) @_fakenlF \times V \frac{\partial y_n}{\partial x_v} @_fakenlF \times O \frac{\partial y_n}{\partial h_n}
Definition at line 557 of file CKalmanFilterCapable.h.
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
If applicable to the given problem, do here any special handling of adding a new landmark to the map.
- Parameters
-
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. |
- See also
- OnInverseObservationModel
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 580 of file CKalmanFilterCapable.h.
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.
Reimplemented in mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 464 of file CKalmanFilterCapable.h.
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
- Parameters
-
in_all_prediction_means | The mean of each landmark predictions; the computation or not of the corresponding covariances is what we're trying to determined with this method. |
out_LM_indices_to_predict | The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted. |
- Note
- This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.
- See also
- OnGetObservations, OnDataAssociation
Definition at line 378 of file CKalmanFilterCapable.h.
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.
Reimplemented in mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 350 of file CKalmanFilterCapable.h.
template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE >
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://www.mrpt.org/Kalman_Filters
Definition at line 27 of file CKalmanFilterCapable_impl.h.