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:
Revisions:
Definition at line 153 of file CKalmanFilterCapable.h.
#include <mrpt/bayes/CKalmanFilterCapable.h>
Public Types | |
typedef KFTYPE | kftype |
The numeric type used in the Kalman Filter (default=double). | |
typedef CKalmanFilterCapable < VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > | KFCLASS |
My class, in a shorter name! | |
typedef mrpt::dynamicsize_vector < KFTYPE > | KFVector |
typedef CMatrixTemplateNumeric < KFTYPE > | KFMatrix |
typedef CMatrixFixedNumeric < KFTYPE, VEH_SIZE, VEH_SIZE > | KFMatrix_VxV |
typedef CMatrixFixedNumeric < KFTYPE, OBS_SIZE, OBS_SIZE > | KFMatrix_OxO |
typedef CMatrixFixedNumeric < KFTYPE, FEAT_SIZE, FEAT_SIZE > | KFMatrix_FxF |
typedef CMatrixFixedNumeric < KFTYPE, ACT_SIZE, ACT_SIZE > | KFMatrix_AxA |
typedef CMatrixFixedNumeric < KFTYPE, VEH_SIZE, OBS_SIZE > | KFMatrix_VxO |
typedef CMatrixFixedNumeric < KFTYPE, VEH_SIZE, FEAT_SIZE > | KFMatrix_VxF |
typedef CMatrixFixedNumeric < KFTYPE, FEAT_SIZE, VEH_SIZE > | KFMatrix_FxV |
typedef CMatrixFixedNumeric < KFTYPE, FEAT_SIZE, OBS_SIZE > | KFMatrix_FxO |
typedef CMatrixFixedNumeric < KFTYPE, OBS_SIZE, FEAT_SIZE > | KFMatrix_OxF |
typedef CMatrixFixedNumeric < KFTYPE, OBS_SIZE, VEH_SIZE > | KFMatrix_OxV |
typedef CArrayNumeric< KFTYPE, VEH_SIZE > | KFArray_VEH |
typedef CArrayNumeric< KFTYPE, ACT_SIZE > | KFArray_ACT |
typedef CArrayNumeric< KFTYPE, OBS_SIZE > | KFArray_OBS |
typedef mrpt::aligned_containers < KFArray_OBS >::vector_t | vector_KFArray_OBS |
typedef CArrayNumeric< KFTYPE, FEAT_SIZE > | KFArray_FEAT |
Public Member Functions | |
size_t | getNumberOfLandmarksInTheMap () const |
bool | isMapEmpty () const |
size_t | getStateVectorLength () const |
void | getLandmarkMean (size_t idx, KFArray_FEAT &feat) const |
Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems). | |
void | getLandmarkCov (size_t idx, KFMatrix_FxF &feat_cov) const |
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems). | |
CKalmanFilterCapable () | |
virtual | ~CKalmanFilterCapable () |
Destructor. | |
mrpt::utils::CTimeLogger & | getProfiler () |
Static Public Member Functions | |
static size_t | get_vehicle_size () |
static size_t | get_observation_size () |
static size_t | get_feature_size () |
static size_t | get_action_size () |
Public Attributes | |
TKF_options | KF_options |
Generic options for the Kalman Filter algorithm 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 (KFArray_ACT &out_u) const =0 |
Must return the action vector u. | |
virtual void | OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const =0 |
Implements the transition model ![]() | |
virtual void | OnTransitionJacobian (KFMatrix_VxV &out_F) const |
Implements the transition Jacobian ![]() | |
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. | |
virtual void | OnTransitionNoise (KFMatrix_VxV &out_Q) const =0 |
Implements the transition noise covariance ![]() | |
virtual void | OnPreComputingPredictions (const vector_KFArray_OBS &in_all_prediction_means, mrpt::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. | |
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. | |
virtual void | OnGetObservationsAndDataAssociation (vector_KFArray_OBS &out_z, mrpt::vector_int &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const 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. | |
virtual void | OnObservationModel (const mrpt::vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const =0 |
Implements the observation prediction ![]() | |
virtual void | OnObservationJacobians (const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const |
Implements the observation Jacobians ![]() ![]() | |
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. | |
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). | |
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". | |
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". | |
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. | |
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(). | |
Protected Attributes | |
mrpt::utils::CTimeLogger | m_timLogger |
Kalman filter state | |
KFVector | m_xkk |
The system state vector. | |
KFMatrix | m_pkk |
The system full covariance matrix. | |
Static Private Member Functions | |
static void | KF_aux_estimate_trans_jacobian (const KFArray_VEH &x, const std::pair< KFCLASS *, KFArray_ACT > &dat, KFArray_VEH &out_x) |
Auxiliary functions for Jacobian numeric estimation. | |
static void | KF_aux_estimate_obs_Hx_jacobian (const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x) |
static void | KF_aux_estimate_obs_Hy_jacobian (const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x) |
Private Attributes | |
vector_KFArray_OBS | all_predictions |
vector_size_t | predictLMidxs |
KFMatrix | dh_dx |
KFMatrix | dh_dx_full |
vector_size_t | idxs |
KFMatrix | S |
KFMatrix | Pkk_subset |
vector_KFArray_OBS | Z |
KFMatrix | K |
KFMatrix | S_1 |
KFMatrix | dh_dx_full_obs |
KFMatrix | aux_K_dh_dx |
bool | m_user_didnt_implement_jacobian |
Friends | |
struct | detail::CRunOneKalmanIteration_addNewLandmarks |
typedef CArrayNumeric<KFTYPE,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_ACT |
Definition at line 186 of file CKalmanFilterCapable.h.
typedef CArrayNumeric<KFTYPE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_FEAT |
Definition at line 189 of file CKalmanFilterCapable.h.
typedef CArrayNumeric<KFTYPE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_OBS |
Definition at line 187 of file CKalmanFilterCapable.h.
typedef CArrayNumeric<KFTYPE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_VEH |
Definition at line 185 of file CKalmanFilterCapable.h.
typedef CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFCLASS |
My class, in a shorter name!
Definition at line 165 of file CKalmanFilterCapable.h.
typedef CMatrixTemplateNumeric<KFTYPE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix |
Definition at line 169 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,ACT_SIZE,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_AxA |
Definition at line 174 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_FxF |
Definition at line 173 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_FxO |
Definition at line 180 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_FxV |
Definition at line 179 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxF |
Definition at line 182 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO |
Definition at line 172 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxV |
Definition at line 183 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_VxF |
Definition at line 177 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_VxO |
Definition at line 176 of file CKalmanFilterCapable.h.
typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_VxV |
Definition at line 171 of file CKalmanFilterCapable.h.
typedef KFTYPE mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::kftype |
The numeric type used in the Kalman Filter (default=double).
Definition at line 164 of file CKalmanFilterCapable.h.
typedef mrpt::dynamicsize_vector<KFTYPE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFVector |
Definition at line 168 of file CKalmanFilterCapable.h.
typedef mrpt::aligned_containers<KFArray_OBS>::vector_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::vector_KFArray_OBS |
Definition at line 188 of file CKalmanFilterCapable.h.
mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::CKalmanFilterCapable | ( | ) | [inline] |
Default constructor
Definition at line 435 of file CKalmanFilterCapable.h.
virtual mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::~CKalmanFilterCapable | ( | ) | [inline, virtual] |
Destructor.
Definition at line 436 of file CKalmanFilterCapable.h.
static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::get_action_size | ( | ) | [inline, static] |
Definition at line 159 of file CKalmanFilterCapable.h.
static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::get_feature_size | ( | ) | [inline, static] |
Definition at line 158 of file CKalmanFilterCapable.h.
static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::get_observation_size | ( | ) | [inline, static] |
Definition at line 157 of file CKalmanFilterCapable.h.
static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::get_vehicle_size | ( | ) | [inline, static] |
Definition at line 156 of file CKalmanFilterCapable.h.
void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getLandmarkCov | ( | size_t | idx, | |
KFMatrix_FxF & | feat_cov | |||
) | const [inline] |
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
std::exception | On idx>= getNumberOfLandmarksInTheMap() |
Definition at line 203 of file CKalmanFilterCapable.h.
void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getLandmarkMean | ( | size_t | idx, | |
KFArray_FEAT & | feat | |||
) | const [inline] |
Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).
std::exception | On idx>= getNumberOfLandmarksInTheMap() |
Definition at line 196 of file CKalmanFilterCapable.h.
size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getNumberOfLandmarksInTheMap | ( | ) | const [inline] |
Definition at line 160 of file CKalmanFilterCapable.h.
mrpt::utils::CTimeLogger& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getProfiler | ( | ) | [inline] |
Definition at line 438 of file CKalmanFilterCapable.h.
size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getStateVectorLength | ( | ) | const [inline] |
Definition at line 191 of file CKalmanFilterCapable.h.
Referenced by mrpt::bayes::detail::isMapEmpty().
bool mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::isMapEmpty | ( | ) | const [inline] |
Definition at line 161 of file CKalmanFilterCapable.h.
static void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KF_aux_estimate_obs_Hx_jacobian | ( | const KFArray_VEH & | x, | |
const std::pair< KFCLASS *, size_t > & | dat, | |||
KFArray_OBS & | out_x | |||
) | [inline, static, private] |
Definition at line 1433 of file CKalmanFilterCapable.h.
static void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KF_aux_estimate_obs_Hy_jacobian | ( | const KFArray_FEAT & | x, | |
const std::pair< KFCLASS *, size_t > & | dat, | |||
KFArray_OBS & | out_x | |||
) | [inline, static, private] |
Definition at line 1446 of file CKalmanFilterCapable.h.
static void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KF_aux_estimate_trans_jacobian | ( | const KFArray_VEH & | x, | |
const std::pair< KFCLASS *, KFArray_ACT > & | dat, | |||
KFArray_VEH & | out_x | |||
) | [inline, static, private] |
Auxiliary functions for Jacobian numeric estimation.
Definition at line 1424 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnGetAction | ( | KFArray_ACT & | out_u | ) | const [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, and mrpt::slam::CRangeBearingKFSLAM2D.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnGetObservationNoise | ( | KFMatrix_OxO & | out_R | ) | const [protected, pure virtual] |
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
out_R | The noise covariance matrix. It might be non diagonal, but it'll usually be. |
Implemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnGetObservationsAndDataAssociation | ( | vector_KFArray_OBS & | out_z, | |
mrpt::vector_int & | out_data_association, | |||
const vector_KFArray_OBS & | in_all_predictions, | |||
const KFMatrix & | in_S, | |||
const vector_size_t & | in_lm_indices_in_S, | |||
const KFMatrix_OxO & | in_R | |||
) | [protected, pure virtual] |
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.
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.
Implemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnInverseObservationModel | ( | const KFArray_OBS & | in_z, | |
KFArray_FEAT & | out_yn, | |||
KFMatrix_FxV & | out_dyn_dxv, | |||
KFMatrix_FxO & | out_dyn_dhn | |||
) | const [inline, 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 | The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation(). | |
out_yn | The F-length vector with the inverse observation model ![]() | |
out_dyn_dxv | The ![]() ![]() | |
out_dyn_dhn | The ![]() ![]() |
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 357 of file CKalmanFilterCapable.h.
Referenced by mrpt::bayes::detail::CRunOneKalmanIteration_addNewLandmarks::operator()().
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::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 [inline, protected, virtual] |
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:
.
but may be computed from additional terms, or whatever needed by the user.
in_z | The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation(). | |
out_yn | The F-length vector with the inverse observation model ![]() | |
out_dyn_dxv | The ![]() ![]() | |
out_dyn_dhn | The ![]() ![]() | |
out_dyn_dhn_R_dyn_dhnT | See the discussion above. |
Definition at line 390 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnNewLandmarkAddedToMap | ( | const size_t | in_obsIdx, | |
const size_t | in_idxNewFeat | |||
) | [inline, protected, virtual] |
If applicable to the given problem, do here any special handling of adding a new landmark to the map.
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. |
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 410 of file CKalmanFilterCapable.h.
Referenced by mrpt::bayes::detail::CRunOneKalmanIteration_addNewLandmarks::operator()().
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnNormalizeStateVector | ( | ) | [inline, 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, and mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 419 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnObservationJacobians | ( | const size_t & | idx_landmark_to_predict, | |
KFMatrix_OxV & | Hx, | |||
KFMatrix_OxF & | Hy | |||
) | const [inline, protected, virtual] |
Implements the observation Jacobians and (when applicable)
.
idx_landmark_to_predict | The index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector. | |
Hx | The output Jacobian ![]() | |
Hy | The output Jacobian ![]() |
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 318 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnObservationJacobiansNumericGetIncrements | ( | KFArray_VEH & | out_veh_increments, | |
KFArray_FEAT & | out_feat_increments | |||
) | const [inline, protected, virtual] |
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 329 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnObservationModel | ( | const mrpt::vector_size_t & | idx_landmarks_to_predict, | |
vector_KFArray_OBS & | out_predictions | |||
) | const [protected, pure virtual] |
Implements the observation prediction .
idx_landmark_to_predict | The indices of the landmarks in the map whose predictions are expected as output. For non SLAM-like problems, this input value is undefined and the application should just generate one observation for the given problem. | |
out_predictions | The predicted observations. |
Implemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnPostIteration | ( | ) | [inline, protected, virtual] |
This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
Definition at line 426 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnPreComputingPredictions | ( | const vector_KFArray_OBS & | in_all_prediction_means, | |
mrpt::vector_size_t & | out_LM_indices_to_predict | |||
) | const [inline, protected, virtual] |
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.
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. |
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 268 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnSubstractObservationVectors | ( | KFArray_OBS & | A, | |
const KFArray_OBS & | B | |||
) | const [inline, protected, virtual] |
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 339 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnTransitionJacobian | ( | KFMatrix_VxV & | out_F | ) | const [inline, protected, virtual] |
Implements the transition Jacobian .
out_F | Must return the Jacobian. The returned matrix must be ![]() |
Reimplemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.
Definition at line 243 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnTransitionJacobianNumericGetIncrements | ( | KFArray_VEH & | out_increments | ) | const [inline, protected, virtual] |
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 250 of file CKalmanFilterCapable.h.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnTransitionModel | ( | const KFArray_ACT & | in_u, | |
KFArray_VEH & | inout_x, | |||
bool & | out_skipPrediction | |||
) | const [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, and mrpt::slam::CRangeBearingKFSLAM2D.
virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnTransitionNoise | ( | KFMatrix_VxV & | out_Q | ) | const [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, and mrpt::slam::CRangeBearingKFSLAM2D.
void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::runOneKalmanIteration | ( | ) | [inline, 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://www.mrpt.org/Kalman_Filters.
Definition at line 466 of file CKalmanFilterCapable.h.
friend struct detail::CRunOneKalmanIteration_addNewLandmarks [friend] |
Definition at line 1462 of file CKalmanFilterCapable.h.
vector_KFArray_OBS mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::all_predictions [private] |
Definition at line 447 of file CKalmanFilterCapable.h.
KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::aux_K_dh_dx [private] |
Definition at line 458 of file CKalmanFilterCapable.h.
KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::dh_dx [private] |
Definition at line 449 of file CKalmanFilterCapable.h.
KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::dh_dx_full [private] |
Definition at line 450 of file CKalmanFilterCapable.h.
KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::dh_dx_full_obs [private] |
Definition at line 457 of file CKalmanFilterCapable.h.
vector_size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::idxs [private] |
Definition at line 451 of file CKalmanFilterCapable.h.
KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::K [private] |
Definition at line 455 of file CKalmanFilterCapable.h.
TKF_options mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KF_options |
Generic options for the Kalman Filter algorithm itself.
Definition at line 440 of file CKalmanFilterCapable.h.
KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_pkk [protected] |
The system full covariance matrix.
Definition at line 212 of file CKalmanFilterCapable.h.
Referenced by mrpt::bayes::detail::CRunOneKalmanIteration_addNewLandmarks::operator()().
mrpt::utils::CTimeLogger mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_timLogger [protected] |
Definition at line 216 of file CKalmanFilterCapable.h.
Referenced by mrpt::bayes::detail::CRunOneKalmanIteration_addNewLandmarks::operator()().
bool mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_user_didnt_implement_jacobian [mutable, private] |
Definition at line 1421 of file CKalmanFilterCapable.h.
KFVector mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_xkk [protected] |
The system state vector.
Definition at line 211 of file CKalmanFilterCapable.h.
Referenced by mrpt::bayes::detail::CRunOneKalmanIteration_addNewLandmarks::operator()().
KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::Pkk_subset [private] |
Definition at line 453 of file CKalmanFilterCapable.h.
vector_size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::predictLMidxs [private] |
Definition at line 448 of file CKalmanFilterCapable.h.
KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::S [private] |
Definition at line 452 of file CKalmanFilterCapable.h.
KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::S_1 [private] |
Definition at line 456 of file CKalmanFilterCapable.h.
vector_KFArray_OBS mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::Z [private] |
Definition at line 454 of file CKalmanFilterCapable.h.
Page generated by Doxygen 1.7.1 for MRPT 0.9.4 SVN: at Mon Jan 10 23:33:19 UTC 2011 |