Detailed Description

An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks.

The main method is "processActionObservation" which processes pairs of action/observation.

The following pages describe front-end applications based on this class:

See also:: CRangeBearingKFSLAM

#include <mrpt/slam/CRangeBearingKFSLAM2D.h>

Inheritance diagram for mrpt::slam::CRangeBearingKFSLAM2D:

[legend]

List of all members.

Classes
struct	TDataAssocInfo
	Information for data-association: More...
struct	TOptions
	The options for the algorithm. More...
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
	CRangeBearingKFSLAM2D ()
	Default constructor.
virtual	~CRangeBearingKFSLAM2D ()
	Destructor.
void	reset ()
	Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
void	processActionObservation (CActionCollectionPtr &action, CSensoryFramePtr &SF)
	Process one new action and observations to update the map and robot pose estimate.
void	getCurrentState (CPosePDFGaussian &out_robotPose, std::vector< TPoint2D > &out_landmarksPositions, std::map< unsigned int, CLandmark::TLandmarkID > &out_landmarkIDs, CVectorDouble &out_fullState, CMatrixDouble &out_fullCovariance) const
	Returns the complete mean and cov.
void	getCurrentRobotPose (CPosePDFGaussian &out_robotPose) const
	Returns the mean & 3x3 covariance matrix of the robot 2D pose.
void	getAs3DObject (mrpt::opengl::CSetOfObjectsPtr &outObj) const
	Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.
void	loadOptions (const mrpt::utils::CConfigFileBase &ini)
	Load options from a ini-like file/text.
void	saveMapAndPath2DRepresentationAsMATLABFile (const std::string &fil, float stdCount=3.0f, const std::string &styleLandmarks=std::string("b"), const std::string &stylePath=std::string("r"), const std::string &styleRobot=std::string("r")) const
	Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D.
const TDataAssocInfo &	getLastDataAssociation () const
	Returns a read-only reference to the information on the last data-association.
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).
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
TOptions	options
	The options for the algorithm.
TKF_options	KF_options
	Generic options for the Kalman Filter algorithm itself.
Protected Member Functions
void	getLandmarkIDsFromIndexInStateVector (std::map< unsigned int, CLandmark::TLandmarkID > &out_id2index) const
void	runOneKalmanIteration ()
	The main entry point, executes one complete step: prediction + update.
Virtual methods for Kalman Filter implementation
void	OnGetAction (KFArray_ACT &out_u) const
	Must return the action vector u.
void	OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
	Implements the transition model $\hat{x}_{k\|k-1} = f( \hat{x}_{k-1\|k-1}, u_k )$ .
void	OnTransitionJacobian (KFMatrix_VxV &out_F) const
	Implements the transition Jacobian $\frac{\partial f}{\partial x}$ .
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.
void	OnTransitionNoise (KFMatrix_VxV &out_Q) const
	Implements the transition noise covariance .
void	OnGetObservationsAndDataAssociation (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)
	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.
void	OnObservationModel (const vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
	Implements the observation prediction .
void	OnObservationJacobians (const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
	Implements the observation Jacobians $\frac{\partial h_i}{\partial x}$ and (when applicable) $\frac{\partial h_i}{\partial y_i}$ .
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.
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).
void	OnGetObservationNoise (KFMatrix_OxO &out_R) const
	Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
void	OnPreComputingPredictions (const vector_KFArray_OBS &in_all_prediction_means, 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.
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".
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.
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 methods for Kalman Filter implementation
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	OnPostIteration ()
	This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
Protected Attributes
CActionCollectionPtr	m_action
	Set up by processActionObservation.
CSensoryFramePtr	m_SF
	Set up by processActionObservation.
mrpt::utils::bimap < CLandmark::TLandmarkID, unsigned int >	m_IDs
	The mapping between landmark IDs and indexes in the Pkk cov.
CSimpleMap	m_SFs
	The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
TDataAssocInfo	m_last_data_association
	Last data association.
mrpt::utils::CTimeLogger	m_timLogger
Kalman filter state
KFVector	m_xkk
	The system state vector.
KFMatrix	m_pkk
	The system full covariance matrix.
Friends
struct	detail::CRunOneKalmanIteration_addNewLandmarks

Member Typedef Documentation

typedef CArrayNumeric<KFTYPE,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable::KFArray_ACT [inherited]

Definition at line 179 of file CKalmanFilterCapable.h.

typedef CArrayNumeric<KFTYPE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable::KFArray_FEAT [inherited]

Definition at line 182 of file CKalmanFilterCapable.h.

typedef CArrayNumeric<KFTYPE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable::KFArray_OBS [inherited]

Definition at line 180 of file CKalmanFilterCapable.h.

typedef CArrayNumeric<KFTYPE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable::KFArray_VEH [inherited]

Definition at line 178 of file CKalmanFilterCapable.h.

typedef CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> mrpt::bayes::CKalmanFilterCapable::KFCLASS [inherited]

My class, in a shorter name!

Definition at line 158 of file CKalmanFilterCapable.h.

typedef CMatrixTemplateNumeric<KFTYPE> mrpt::bayes::CKalmanFilterCapable::KFMatrix [inherited]

Definition at line 162 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,ACT_SIZE,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_AxA [inherited]

Definition at line 167 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_FxF [inherited]

Definition at line 166 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_FxO [inherited]

Definition at line 173 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_FxV [inherited]

Definition at line 172 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_OxF [inherited]

Definition at line 175 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_OxO [inherited]

Definition at line 165 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_OxV [inherited]

Definition at line 176 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_VxF [inherited]

Definition at line 170 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_VxO [inherited]

Definition at line 169 of file CKalmanFilterCapable.h.

typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable::KFMatrix_VxV [inherited]

Definition at line 164 of file CKalmanFilterCapable.h.

typedef KFTYPE mrpt::bayes::CKalmanFilterCapable::kftype [inherited]

The numeric type used in the Kalman Filter (default=double)

Definition at line 157 of file CKalmanFilterCapable.h.

typedef mrpt::dynamicsize_vector<KFTYPE> mrpt::bayes::CKalmanFilterCapable::KFVector [inherited]

Definition at line 161 of file CKalmanFilterCapable.h.

typedef mrpt::aligned_containers<KFArray_OBS>::vector_t mrpt::bayes::CKalmanFilterCapable::vector_KFArray_OBS [inherited]

Definition at line 181 of file CKalmanFilterCapable.h.

Constructor & Destructor Documentation

mrpt::slam::CRangeBearingKFSLAM2D::CRangeBearingKFSLAM2D ( )

Default constructor.

virtual mrpt::slam::CRangeBearingKFSLAM2D::~CRangeBearingKFSLAM2D ( ) [virtual]

Destructor.

Member Function Documentation

static size_t mrpt::bayes::CKalmanFilterCapable::get_action_size ( ) [inline, static, inherited]

Definition at line 152 of file CKalmanFilterCapable.h.

static size_t mrpt::bayes::CKalmanFilterCapable::get_feature_size ( ) [inline, static, inherited]

Definition at line 151 of file CKalmanFilterCapable.h.

static size_t mrpt::bayes::CKalmanFilterCapable::get_observation_size ( ) [inline, static, inherited]

Definition at line 150 of file CKalmanFilterCapable.h.

static size_t mrpt::bayes::CKalmanFilterCapable::get_vehicle_size ( ) [inline, static, inherited]

Definition at line 149 of file CKalmanFilterCapable.h.

void mrpt::slam::CRangeBearingKFSLAM2D::getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr & outObj ) const

Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.

Parameters:

out_objects

void mrpt::slam::CRangeBearingKFSLAM2D::getCurrentRobotPose ( CPosePDFGaussian & out_robotPose ) const

Returns the mean & 3x3 covariance matrix of the robot 2D pose.

See also:: getCurrentState

void mrpt::slam::CRangeBearingKFSLAM2D::getCurrentState	(	CPosePDFGaussian &	out_robotPose,
		std::vector< TPoint2D > &	out_landmarksPositions,
		std::map< unsigned int, CLandmark::TLandmarkID > &	out_landmarkIDs,
		CVectorDouble &	out_fullState,
		CMatrixDouble &	out_fullCovariance
	)		const

Returns the complete mean and cov.

Parameters:

out_robotPose	The mean & 3x3 covariance matrix of the robot 2D pose
out_landmarksPositions	One entry for each of the M landmark positions (2D).
out_landmarkIDs	Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullState	The complete state vector (3+2M).
out_fullCovariance	The full (3+2M)x(3+2M) covariance matrix of the filter.

See also:: getCurrentRobotPose

void mrpt::bayes::CKalmanFilterCapable::getLandmarkCov	(	size_t	idx,
		KFMatrix_FxF &	feat_cov
	)		const `[inline, inherited]`

Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).

Exceptions:

std::exception On idx>= getNumberOfLandmarksInTheMap()

Definition at line 196 of file CKalmanFilterCapable.h.

void mrpt::slam::CRangeBearingKFSLAM2D::getLandmarkIDsFromIndexInStateVector ( std::map< unsigned int, CLandmark::TLandmarkID > & out_id2index ) const [inline, protected]

Definition at line 335 of file CRangeBearingKFSLAM2D.h.

void mrpt::bayes::CKalmanFilterCapable::getLandmarkMean	(	size_t	idx,
		KFArray_FEAT &	feat
	)		const `[inline, inherited]`

Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).

Exceptions:

std::exception On idx>= getNumberOfLandmarksInTheMap()

Definition at line 189 of file CKalmanFilterCapable.h.

const TDataAssocInfo& mrpt::slam::CRangeBearingKFSLAM2D::getLastDataAssociation ( ) const [inline]

Returns a read-only reference to the information on the last data-association.

Definition at line 194 of file CRangeBearingKFSLAM2D.h.

size_t mrpt::bayes::CKalmanFilterCapable::getNumberOfLandmarksInTheMap ( ) const [inline, inherited]

Definition at line 153 of file CKalmanFilterCapable.h.

mrpt::utils::CTimeLogger& mrpt::bayes::CKalmanFilterCapable::getProfiler ( ) [inline, inherited]

Definition at line 431 of file CKalmanFilterCapable.h.

size_t mrpt::bayes::CKalmanFilterCapable::getStateVectorLength ( ) const [inline, inherited]

Definition at line 184 of file CKalmanFilterCapable.h.

bool mrpt::bayes::CKalmanFilterCapable::isMapEmpty ( ) const [inline, inherited]

Definition at line 154 of file CKalmanFilterCapable.h.

void mrpt::slam::CRangeBearingKFSLAM2D::loadOptions ( const mrpt::utils::CConfigFileBase & ini )

Load options from a ini-like file/text.

void mrpt::slam::CRangeBearingKFSLAM2D::OnGetAction ( KFArray_ACT & out_u ) const [protected, virtual]

Must return the action vector u.

Parameters:

out_u The action vector which will be passed to OnTransitionModel

Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnGetObservationNoise ( KFMatrix_OxO & out_R ) const [protected, 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.

Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnGetObservationsAndDataAssociation	(	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, 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_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.

Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnInverseObservationModel	(	const KFArray_OBS &	in_z,
		KFArray_FEAT &	out_yn,
		KFMatrix_FxV &	out_dyn_dxv,
		KFMatrix_FxO &	out_dyn_dhn
	)		const `[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 OnGetObservations().
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 from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

virtual void mrpt::bayes::CKalmanFilterCapable::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, inherited]`

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:

$\frac{\partial y_n}{\partial h_n} R \frac{\partial y_n}{\partial h_n}^\top$ .

but may be computed from additional terms, or whatever needed by the user.

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}$ .
out_dyn_dhn_R_dyn_dhnT	See the discussion above.

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.

Definition at line 383 of file CKalmanFilterCapable.h.

void mrpt::slam::CRangeBearingKFSLAM2D::OnNewLandmarkAddedToMap	(	const size_t	in_obsIdx,
		const size_t	in_idxNewFeat
	)		`[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 from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::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 from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnObservationJacobians	(	const size_t &	idx_landmark_to_predict,
		KFMatrix_OxV &	Hx,
		KFMatrix_OxF &	Hy
	)		const `[protected, 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}$ .

Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnObservationJacobiansNumericGetIncrements	(	KFArray_VEH &	out_veh_increments,
		KFArray_FEAT &	out_feat_increments
	)		const `[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 from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnObservationModel	(	const vector_size_t &	idx_landmarks_to_predict,
		vector_KFArray_OBS &	out_predictions
	)		const `[protected, 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.

Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

virtual void mrpt::bayes::CKalmanFilterCapable::OnPostIteration ( ) [inline, protected, virtual, inherited]

This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().

Definition at line 419 of file CKalmanFilterCapable.h.

void mrpt::slam::CRangeBearingKFSLAM2D::OnPreComputingPredictions	(	const vector_KFArray_OBS &	in_all_prediction_means,
		vector_size_t &	out_LM_indices_to_predict
	)		const `[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 from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnSubstractObservationVectors	(	KFArray_OBS &	A,
		const KFArray_OBS &	B
	)		const `[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 from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionJacobian ( KFMatrix_VxV & out_F ) const [protected, 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).

Reimplemented from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionJacobianNumericGetIncrements ( KFArray_VEH & out_increments ) const [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 from mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionModel	(	const KFArray_ACT &	in_u,
		KFArray_VEH &	inout_x,
		bool &	out_skipPrediction
	)		const `[protected, 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

Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionNoise ( KFMatrix_VxV & out_Q ) const [protected, 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

Implements mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >.

void mrpt::slam::CRangeBearingKFSLAM2D::processActionObservation	(	CActionCollectionPtr &	action,
		CSensoryFramePtr &	SF
	)

Process one new action and observations to update the map and robot pose estimate.

See the description of the class at the top of this page.

Parameters:

action	May contain odometry
SF	The set of observations, must contain at least one CObservationBearingRange

void mrpt::slam::CRangeBearingKFSLAM2D::reset ( )

Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).

void mrpt::bayes::CKalmanFilterCapable::runOneKalmanIteration ( ) [inline, protected, inherited]

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

void mrpt::slam::CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile	(	const std::string &	fil,
		float	stdCount = `3.0f`,
		const std::string &	styleLandmarks = `std::string("b")`,
		const std::string &	stylePath = `std::string("r")`,
		const std::string &	styleRobot = `std::string("r")`
	)		const