mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > Class Template Reference

Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations. More...

#include <mrpt/bayes/CKalmanFilterCapable.h>

Inheritance diagram for mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >:

[legend]

Collaboration diagram for mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >:

[legend]

List of all members.

Public Types
typedef KFTYPE	kftype
	The numeric type used in the Kalman Filter (default=double).
typedef CVectorTemplate< 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 CArrayNumeric< KFTYPE, FEAT_SIZE >	KFArray_FEAT
Public Member Functions
size_t	getNumberOfLandmarksInTheMap () const
	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)=0
	Must return the action vector u.
virtual void	OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction)=0
	Implements the transition model $\hat{x}_{k\|k-1} = f( \hat{x}_{k-1\|k-1}, u_k )$ .
virtual void	OnTransitionJacobian (KFMatrix_VxV &out_F)=0
	Implements the transition Jacobian $\frac{\partial f}{\partial x}$ .
virtual void	OnTransitionNoise (KFMatrix_VxV &out_Q)=0
	Implements the transition noise covariance .
virtual void	OnPreComputingPredictions (const vector< KFArray_OBS > &in_all_prediction_means, vector_size_t &out_LM_indices_to_predict)
	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)=0
	Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
virtual void	OnGetObservationsAndDataAssociation (std::vector< KFArray_OBS > &out_z, 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 vector_size_t &idx_landmarks_to_predict, std::vector< KFArray_OBS > &out_predictions)=0
	Implements the observation prediction .
virtual void	OnObservationJacobians (const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy)=0
	Implements the observation Jacobians $\frac{\partial h_i}{\partial x}$ and (when applicable) $\frac{\partial h_i}{\partial y_i}$ .
virtual void	OnSubstractObservationVectors (KFArray_OBS &A, const KFArray_OBS &B)
	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)
	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.
Friends
template<size_t _VEH_SIZE, size_t _OBS_SIZE, size_t _FEAT_SIZE, size_t _ACT_SIZE, typename _KFTYPE >
void	detail::runOneKalmanIteration_addNewLandmarks (CKalmanFilterCapable< _VEH_SIZE, _OBS_SIZE, _FEAT_SIZE, _ACT_SIZE, _KFTYPE > &obj, std::vector< typename CKalmanFilterCapable< _VEH_SIZE, _OBS_SIZE, _FEAT_SIZE, _ACT_SIZE, _KFTYPE >::KFArray_OBS > Z, const vector_int &data_association, const typename CKalmanFilterCapable< _VEH_SIZE, _OBS_SIZE, _FEAT_SIZE, _ACT_SIZE, _KFTYPE >::KFMatrix_OxO &R)

Detailed Description

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://babel.isa.uma.es/mrpt/index.php/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 142 of file CKalmanFilterCapable.h.

Member Typedef Documentation

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CArrayNumeric<KFTYPE,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_ACT

Definition at line 173 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CArrayNumeric<KFTYPE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_FEAT

Definition at line 175 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CArrayNumeric<KFTYPE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_OBS

Definition at line 174 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CArrayNumeric<KFTYPE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_VEH

Definition at line 172 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixTemplateNumeric<KFTYPE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix

Definition at line 156 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,ACT_SIZE,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_AxA

Definition at line 161 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_FxF

Definition at line 160 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_FxO

Definition at line 167 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_FxV

Definition at line 166 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxF

Definition at line 169 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO

Definition at line 159 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxV

Definition at line 170 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_VxF

Definition at line 164 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_VxO

Definition at line 163 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_VxV

Definition at line 158 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

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 152 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

typedef CVectorTemplate<KFTYPE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFVector

Definition at line 155 of file CKalmanFilterCapable.h.

Constructor & Destructor Documentation

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::CKalmanFilterCapable ( ) [inline]

Default constructor

Definition at line 343 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

virtual mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::~CKalmanFilterCapable ( ) [inline, virtual]

Destructor.

Definition at line 344 of file CKalmanFilterCapable.h.

Member Function Documentation

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::get_action_size ( ) [inline, static]

Definition at line 148 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::get_feature_size ( ) [inline, static]

Definition at line 147 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::get_observation_size ( ) [inline, static]

Definition at line 146 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::get_vehicle_size ( ) [inline, static]

Definition at line 145 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getNumberOfLandmarksInTheMap ( ) const [inline]

Definition at line 149 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::OnPreComputingPredictions(), and mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

mrpt::utils::CTimeLogger& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getProfiler ( ) [inline]

Definition at line 346 of file CKalmanFilterCapable.h.

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnGetAction ( KFArray_ACT & out_u ) [protected, pure virtual]

Must return the action vector u.

Parameters:

out_u

The action vector which will be passed to OnTransitionModel

Implemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnGetObservationNoise ( KFMatrix_OxO & out_R ) [protected, pure virtual]

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, and mrpt::slam::CRangeBearingKFSLAM2D.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnGetObservationsAndDataAssociation	(	std::vector< KFArray_OBS > &	out_z,
		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.

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.

Implemented in mrpt::slam::CRangeBearingKFSLAM.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

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
	)			`[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".

Parameters:

	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 $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $\frac{\partial y_n}{\partial x_v}$ .
	out_dyn_dhn	The $F \times O$ Jacobian of the inv. sensor model wrt the observation vector $\frac{\partial y_n}{\partial h_n}$ .

O: OBS_SIZE
V: VEH_SIZE
F: FEAT_SIZE

Note:: OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.

Reimplemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.

Definition at line 302 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::detail::runOneKalmanIteration_addNewLandmarks().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

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.

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 318 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::detail::runOneKalmanIteration_addNewLandmarks().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

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 327 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

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
	)			`[protected, pure virtual]`

Implements the observation Jacobians $\frac{\partial h_i}{\partial x}$ and (when applicable) $\frac{\partial h_i}{\partial y_i}$ .

Parameters:

	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 $\frac{\partial h_i}{\partial x}$ .
	Hy	The output Jacobian $\frac{\partial h_i}{\partial y_i}$ .

Implemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnObservationModel	(	const vector_size_t &	idx_landmarks_to_predict,
		std::vector< KFArray_OBS > &	out_predictions
	)			`[protected, pure virtual]`

Implements the observation prediction $ h_i(x) $ .

Parameters:

	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.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

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 334 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnPreComputingPredictions	(	const vector< KFArray_OBS > &	in_all_prediction_means,
		vector_size_t &	out_LM_indices_to_predict
	)			`[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.

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

Reimplemented in mrpt::slam::CRangeBearingKFSLAM.

Definition at line 227 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnSubstractObservationVectors	(	KFArray_OBS &	A,
		const KFArray_OBS &	B
	)			`[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 285 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnTransitionJacobian ( KFMatrix_VxV & out_F ) [protected, pure virtual]

Implements the transition Jacobian $\frac{\partial f}{\partial x}$ .

Parameters:

out_F

Must return the Jacobian. The returned matrix must be $V \times V$ with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).

Implemented in mrpt::slam::CRangeBearingKFSLAM, and mrpt::slam::CRangeBearingKFSLAM2D.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

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
	)			`[protected, pure virtual]`

Implements the transition model $\hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k )$ .

Parameters:

in_u

The vector returned by OnGetAction.

inout_x

At input has

$\hat{x}_{k-1|k-1}$

, at output must have $\hat{x}_{k|k-1}$ .

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.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnTransitionNoise ( KFMatrix_VxV & out_Q ) [protected, pure virtual]

Implements the transition noise covariance $ Q_k $ .

Parameters:

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.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

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://babel.isa.uma.es/mrpt/index.php/Kalman_Filters.

Definition at line 356 of file CKalmanFilterCapable.h.

Friends And Related Function Documentation

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 >

void detail::runOneKalmanIteration_addNewLandmarks	(	CKalmanFilterCapable< _VEH_SIZE, _OBS_SIZE, _FEAT_SIZE, _ACT_SIZE, _KFTYPE > &	obj,
		std::vector< typename CKalmanFilterCapable< _VEH_SIZE, _OBS_SIZE, _FEAT_SIZE, _ACT_SIZE, _KFTYPE >::KFArray_OBS >	Z,
		const vector_int &	data_association,
		const typename CKalmanFilterCapable< _VEH_SIZE, _OBS_SIZE, _FEAT_SIZE, _ACT_SIZE, _KFTYPE >::KFMatrix_OxO &	R
	)			`[friend]`

Member Data Documentation

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

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 348 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_pkk [protected]

The system full covariance matrix.

Definition at line 182 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration(), and mrpt::bayes::detail::runOneKalmanIteration_addNewLandmarks().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

mrpt::utils::CTimeLogger mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_timLogger [protected]

Definition at line 186 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::getProfiler(), mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration(), and mrpt::bayes::detail::runOneKalmanIteration_addNewLandmarks().

template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>

KFVector mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_xkk [protected]

The system state vector.

Definition at line 181 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::getNumberOfLandmarksInTheMap(), mrpt::bayes::CKalmanFilterCapable< 6, 3, 3, 6 >::runOneKalmanIteration(), and mrpt::bayes::detail::runOneKalmanIteration_addNewLandmarks().

mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > Class Template Reference

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Friends

Detailed Description

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 >

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation

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 >