Main MRPT website > C++ reference for MRPT 1.5.8
List of all members | Classes | Public Types | Public Member Functions | Static Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes
mrpt::slam::CRangeBearingKFSLAM2D Class Referenceabstract

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

Definition at line 45 of file CRangeBearingKFSLAM2D.h.

#include <mrpt/slam/CRangeBearingKFSLAM2D.h>

Inheritance diagram for mrpt::slam::CRangeBearingKFSLAM2D:
Inheritance graph

Classes

struct  TDataAssocInfo
 Information for data-association: More...
 
struct  TOptions
 The options for the algorithm. More...
 

Public Types

typedef mrpt::math::TPoint2D landmark_point_t
 Either mrpt::math::TPoint2D or mrpt::math::TPoint3D. More...
 
typedef double kftype
 The numeric type used in the Kalman Filter (default=double) More...
 
typedef CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double > KFCLASS
 My class, in a shorter name! More...
 
typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > KFVector
 
typedef mrpt::math::CMatrixTemplateNumeric< double > KFMatrix
 
typedef mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
 
typedef mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
 
typedef mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, FEAT_SIZE > KFMatrix_FxF
 
typedef mrpt::math::CMatrixFixedNumeric< double, ACT_SIZE, ACT_SIZE > KFMatrix_AxA
 
typedef mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, OBS_SIZE > KFMatrix_VxO
 
typedef mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, FEAT_SIZE > KFMatrix_VxF
 
typedef mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
 
typedef mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
 
typedef mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
 
typedef mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
 
typedef mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
 
typedef mrpt::math::CArrayNumeric< double, ACT_SIZE > KFArray_ACT
 
typedef mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
 
typedef mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
 
typedef mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
 

Public Member Functions

 CRangeBearingKFSLAM2D ()
 Default constructor. More...
 
virtual ~CRangeBearingKFSLAM2D ()
 Destructor. More...
 
void reset ()
 Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0). More...
 
void processActionObservation (mrpt::obs::CActionCollectionPtr &action, mrpt::obs::CSensoryFramePtr &SF)
 Process one new action and observations to update the map and robot pose estimate. More...
 
void getCurrentState (mrpt::poses::CPosePDFGaussian &out_robotPose, std::vector< mrpt::math::TPoint2D > &out_landmarksPositions, std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_landmarkIDs, mrpt::math::CVectorDouble &out_fullState, mrpt::math::CMatrixDouble &out_fullCovariance) const
 Returns the complete mean and cov. More...
 
void getCurrentRobotPose (mrpt::poses::CPosePDFGaussian &out_robotPose) const
 Returns the mean & 3x3 covariance matrix of the robot 2D pose. More...
 
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. More...
 
void loadOptions (const mrpt::utils::CConfigFileBase &ini)
 Load options from a ini-like file/text. More...
 
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. More...
 
const TDataAssocInfogetLastDataAssociation () const
 Returns a read-only reference to the information on the last data-association. More...
 
size_t getNumberOfLandmarksInTheMap () const
 
bool isMapEmpty () const
 
size_t getStateVectorLength () const
 
KFVectorinternal_getXkk ()
 
KFMatrixinternal_getPkk ()
 
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). More...
 
void getLandmarkCov (size_t idx, KFMatrix_FxF &feat_cov) const
 Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems). More...
 
mrpt::utils::CTimeLoggergetProfiler ()
 

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. More...
 
TKF_options KF_options
 Generic options for the Kalman Filter algorithm itself. More...
 

Protected Member Functions

void getLandmarkIDsFromIndexInStateVector (std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_id2index) const
 
void runOneKalmanIteration ()
 The main entry point, executes one complete step: prediction + update. More...
 
Virtual methods for Kalman Filter implementation
void OnGetAction (KFArray_ACT &out_u) const
 Must return the action vector u. More...
 
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 ) $. More...
 
void OnTransitionJacobian (KFMatrix_VxV &out_F) const
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $. More...
 
void OnTransitionJacobianNumericGetIncrements (KFArray_VEH &out_increments) const
 Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
 
void OnTransitionNoise (KFMatrix_VxV &out_Q) const
 Implements the transition noise covariance $ Q_k $. More...
 
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. More...
 
void OnObservationModel (const vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
 
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} $. More...
 
void OnObservationJacobiansNumericGetIncrements (KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const
 Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
 
void OnSubstractObservationVectors (KFArray_OBS &A, const KFArray_OBS &B) const
 Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles). More...
 
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. More...
 
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. More...
 
void OnInverseObservationModel (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
 
void OnNewLandmarkAddedToMap (const size_t in_obsIdx, const size_t in_idxNewFeat)
 If applicable to the given problem, do here any special handling of adding a new landmark to the map. More...
 
void OnNormalizeStateVector ()
 This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it. More...
 

Protected Attributes

mrpt::obs::CActionCollectionPtr m_action
 Set up by processActionObservation. More...
 
mrpt::obs::CSensoryFramePtr m_SF
 Set up by processActionObservation. More...
 
mrpt::utils::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
 The mapping between landmark IDs and indexes in the Pkk cov. More...
 
mrpt::maps::CSimpleMap m_SFs
 The sequence of all the observations and the robot path (kept for debugging, statistics,etc) More...
 
TDataAssocInfo m_last_data_association
 Last data association. More...
 
mrpt::utils::CTimeLogger m_timLogger
 
Kalman filter state
KFVector m_xkk
 The system state vector. More...
 
KFMatrix m_pkk
 The system full covariance matrix. More...
 

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) const
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
 
virtual void OnInverseObservationModel (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn, KFMatrix_FxF &out_dyn_dhn_R_dyn_dhnT, bool &out_use_dyn_dhn_jacobian) const
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
 
virtual void OnPostIteration ()
 This method is called after finishing one KF iteration and before returning from runOneKalmanIteration(). More...
 
virtual void OnGetAction (KFArray_ACT &out_u) const=0
 Must return the action vector u. More...
 
virtual void OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const=0
 Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $. More...
 
virtual void OnTransitionJacobian (KFMatrix_VxV &out_F) const
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $. More...
 
virtual void OnTransitionJacobianNumericGetIncrements (KFArray_VEH &out_increments) const
 Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
 
virtual void OnTransitionNoise (KFMatrix_VxV &out_Q) const=0
 Implements the transition noise covariance $ Q_k $. More...
 
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. More...
 
virtual void OnGetObservationNoise (KFMatrix_OxO &out_R) const=0
 Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. More...
 
virtual void OnGetObservationsAndDataAssociation (vector_KFArray_OBS &out_z, 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. More...
 
virtual void OnObservationModel (const mrpt::vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const=0
 Implements the observation prediction $ h_i(x) $. More...
 
virtual 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} $. More...
 
virtual void OnObservationJacobiansNumericGetIncrements (KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const
 Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
 
virtual void OnSubstractObservationVectors (KFArray_OBS &A, const KFArray_OBS &B) const
 Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles). More...
 

Member Typedef Documentation

◆ KFArray_ACT

typedef mrpt::math::CArrayNumeric<double ,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_ACT
inherited

Definition at line 193 of file CKalmanFilterCapable.h.

◆ KFArray_FEAT

typedef mrpt::math::CArrayNumeric<double ,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_FEAT
inherited

Definition at line 196 of file CKalmanFilterCapable.h.

◆ KFArray_OBS

typedef mrpt::math::CArrayNumeric<double ,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_OBS
inherited

Definition at line 194 of file CKalmanFilterCapable.h.

◆ KFArray_VEH

typedef mrpt::math::CArrayNumeric<double ,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_VEH
inherited

Definition at line 192 of file CKalmanFilterCapable.h.

◆ KFCLASS

typedef CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,double > mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFCLASS
inherited

My class, in a shorter name!

Definition at line 172 of file CKalmanFilterCapable.h.

◆ KFMatrix

typedef mrpt::math::CMatrixTemplateNumeric<double > mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix
inherited

Definition at line 176 of file CKalmanFilterCapable.h.

◆ KFMatrix_AxA

typedef mrpt::math::CMatrixFixedNumeric<double ,ACT_SIZE,ACT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_AxA
inherited

Definition at line 181 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxF

typedef mrpt::math::CMatrixFixedNumeric<double ,FEAT_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxF
inherited

Definition at line 180 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxO

typedef mrpt::math::CMatrixFixedNumeric<double ,FEAT_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxO
inherited

Definition at line 187 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxV

typedef mrpt::math::CMatrixFixedNumeric<double ,FEAT_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxV
inherited

Definition at line 186 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxF

typedef mrpt::math::CMatrixFixedNumeric<double ,OBS_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxF
inherited

Definition at line 189 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxO

typedef mrpt::math::CMatrixFixedNumeric<double ,OBS_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxO
inherited

Definition at line 179 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxV

typedef mrpt::math::CMatrixFixedNumeric<double ,OBS_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxV
inherited

Definition at line 190 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxF

typedef mrpt::math::CMatrixFixedNumeric<double ,VEH_SIZE,FEAT_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxF
inherited

Definition at line 184 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxO

typedef mrpt::math::CMatrixFixedNumeric<double ,VEH_SIZE,OBS_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxO
inherited

Definition at line 183 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxV

typedef mrpt::math::CMatrixFixedNumeric<double ,VEH_SIZE,VEH_SIZE> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxV
inherited

Definition at line 178 of file CKalmanFilterCapable.h.

◆ kftype

typedef double mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::kftype
inherited

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

Definition at line 171 of file CKalmanFilterCapable.h.

◆ KFVector

typedef Eigen::Matrix<double ,Eigen::Dynamic,1> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFVector
inherited

Definition at line 175 of file CKalmanFilterCapable.h.

◆ landmark_point_t

Either mrpt::math::TPoint2D or mrpt::math::TPoint3D.

Definition at line 50 of file CRangeBearingKFSLAM2D.h.

◆ vector_KFArray_OBS

typedef mrpt::aligned_containers<KFArray_OBS>::vector_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::vector_KFArray_OBS
inherited

Definition at line 195 of file CKalmanFilterCapable.h.

Constructor & Destructor Documentation

◆ CRangeBearingKFSLAM2D()

CRangeBearingKFSLAM2D::CRangeBearingKFSLAM2D ( )

Default constructor.

Definition at line 42 of file CRangeBearingKFSLAM2D.cpp.

References reset().

◆ ~CRangeBearingKFSLAM2D()

CRangeBearingKFSLAM2D::~CRangeBearingKFSLAM2D ( )
virtual

Destructor.

Definition at line 72 of file CRangeBearingKFSLAM2D.cpp.

Member Function Documentation

◆ get_action_size()

static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_action_size ( )
inlinestaticinherited

Definition at line 166 of file CKalmanFilterCapable.h.

◆ get_feature_size()

static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_feature_size ( )
inlinestaticinherited

◆ get_observation_size()

static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_observation_size ( )
inlinestaticinherited

Definition at line 164 of file CKalmanFilterCapable.h.

Referenced by OnGetObservationsAndDataAssociation().

◆ get_vehicle_size()

static size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_vehicle_size ( )
inlinestaticinherited

◆ getAs3DObject()

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

◆ getCurrentRobotPose()

void CRangeBearingKFSLAM2D::getCurrentRobotPose ( mrpt::poses::CPosePDFGaussian out_robotPose) const

◆ getCurrentState()

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

Returns the complete mean and cov.

Parameters
out_robotPoseThe mean & 3x3 covariance matrix of the robot 2D pose
out_landmarksPositionsOne entry for each of the M landmark positions (2D).
out_landmarkIDsEach element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullStateThe complete state vector (3+2M).
out_fullCovarianceThe full (3+2M)x(3+2M) covariance matrix of the filter.
See also
getCurrentRobotPose

Definition at line 101 of file CRangeBearingKFSLAM2D.cpp.

References ASSERT_, mrpt::poses::CPosePDFGaussian::cov, mrpt::utils::bimap< KEY, VALUE >::getInverseMap(), m_IDs, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, mrpt::poses::CPosePDFGaussian::mean, MRPT_END, and MRPT_START.

◆ getLandmarkCov()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getLandmarkCov ( size_t  idx,
KFMatrix_FxF feat_cov 
) const
inlineinherited

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

Exceptions
std::exceptionOn idx>= getNumberOfLandmarksInTheMap()

Definition at line 213 of file CKalmanFilterCapable.h.

◆ getLandmarkIDsFromIndexInStateVector()

void mrpt::slam::CRangeBearingKFSLAM2D::getLandmarkIDsFromIndexInStateVector ( std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &  out_id2index) const
inlineprotected

Definition at line 302 of file CRangeBearingKFSLAM2D.h.

◆ getLandmarkMean()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getLandmarkMean ( size_t  idx,
KFArray_FEAT feat 
) const
inlineinherited

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

Exceptions
std::exceptionOn idx>= getNumberOfLandmarksInTheMap()

Definition at line 206 of file CKalmanFilterCapable.h.

◆ getLastDataAssociation()

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

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

Definition at line 162 of file CRangeBearingKFSLAM2D.h.

◆ getNumberOfLandmarksInTheMap()

size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getNumberOfLandmarksInTheMap ( ) const
inlineinherited

Definition at line 167 of file CKalmanFilterCapable.h.

Referenced by OnObservationModel(), and processActionObservation().

◆ getProfiler()

mrpt::utils::CTimeLogger& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getProfiler ( )
inlineinherited

Definition at line 460 of file CKalmanFilterCapable.h.

◆ getStateVectorLength()

size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getStateVectorLength ( ) const
inlineinherited

Definition at line 198 of file CKalmanFilterCapable.h.

◆ internal_getPkk()

KFMatrix& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::internal_getPkk ( )
inlineinherited

Definition at line 201 of file CKalmanFilterCapable.h.

◆ internal_getXkk()

KFVector& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::internal_getXkk ( )
inlineinherited

Definition at line 200 of file CKalmanFilterCapable.h.

◆ isMapEmpty()

bool mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::isMapEmpty ( ) const
inlineinherited

Definition at line 168 of file CKalmanFilterCapable.h.

◆ loadOptions()

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

◆ OnGetAction() [1/2]

void CRangeBearingKFSLAM2D::OnGetAction ( KFArray_ACT u) const
protected

Must return the action vector u.

Parameters
out_uThe action vector which will be passed to OnTransitionModel

Definition at line 183 of file CRangeBearingKFSLAM2D.cpp.

References m_action, mrpt::poses::CPose3DPDFGaussian::mean, mrpt::poses::CPose2D::phi(), mrpt::obs::CActionRobotMovement3D::poseChange, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().

◆ OnGetAction() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnGetAction ( KFArray_ACT out_u) const
protectedpure virtualinherited

Must return the action vector u.

Parameters
out_uThe action vector which will be passed to OnTransitionModel

◆ OnGetObservationNoise() [1/2]

void CRangeBearingKFSLAM2D::OnGetObservationNoise ( KFMatrix_OxO out_R) const
protected

Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.

Parameters
out_RThe noise covariance matrix. It might be non diagonal, but it'll usually be.

Definition at line 1031 of file CRangeBearingKFSLAM2D.cpp.

References options, mrpt::math::square(), mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_range, and mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_yaw.

◆ OnGetObservationNoise() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnGetObservationNoise ( KFMatrix_OxO out_R) const
protectedpure virtualinherited

Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.

Parameters
out_RThe 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.

◆ OnGetObservationsAndDataAssociation() [1/2]

void CRangeBearingKFSLAM2D::OnGetObservationsAndDataAssociation ( vector_KFArray_OBS Z,
vector_int data_association,
const vector_KFArray_OBS all_predictions,
const KFMatrix S,
const vector_size_t lm_indices_in_S,
const KFMatrix_OxO R 
)
protected

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_zN 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_associationAn 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_SThe 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_SThe 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.
Parameters
out_zN 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_associationAn 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_SThe 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_SThe 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.

Definition at line 491 of file CRangeBearingKFSLAM2D.cpp.

References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::all_predictions, ASSERTMSG_, mrpt::slam::TDataAssociationResults::associations, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::clear(), mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_IC_chi2_thres, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_IC_metric, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_IC_ml_threshold, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_method, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_metric, mrpt::slam::data_association_full_covariance(), mrpt::utils::bimap< KEY, VALUE >::end(), mrpt::utils::find_in_vector(), mrpt::utils::bimap< KEY, VALUE >::find_key(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_observation_size(), m_IDs, m_last_data_association, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, m_SF, mrpt::math::mahalanobisDistance2AndLogPDF(), MRPT_END, MRPT_START, options, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::predictions_IDs, mrpt::utils::CStream::printf(), R, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::results, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::S, mrpt::math::UNINITIALIZED_MATRIX, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::Y_pred_covs, mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::Y_pred_means, and mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::Z.

◆ OnGetObservationsAndDataAssociation() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::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 
)
protectedpure virtualinherited

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_zN 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_associationAn 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_predictionsA 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_SThe 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_SThe 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.

◆ OnInverseObservationModel() [1/3]

void CRangeBearingKFSLAM2D::OnInverseObservationModel ( const KFArray_OBS in_z,
KFArray_FEAT out_yn,
KFMatrix_FxV out_dyn_dxv,
KFMatrix_FxO out_dyn_dhn 
) const
protected

If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".

Parameters
in_zThe observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().
out_ynThe F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
out_dyn_dxvThe $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $.
out_dyn_dhnThe $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.

Definition at line 755 of file CRangeBearingKFSLAM2D.cpp.

References ASSERTMSG_, m_SF, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, and MRPT_START.

◆ OnInverseObservationModel() [2/3]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnInverseObservationModel ( const KFArray_OBS in_z,
KFArray_FEAT out_yn,
KFMatrix_FxV out_dyn_dxv,
KFMatrix_FxO out_dyn_dhn 
) const
inlinevirtualinherited

If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".

Parameters
in_zThe observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
out_ynThe F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
out_dyn_dxvThe $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $.
out_dyn_dhnThe $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.
Deprecated:
This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.

Definition at line 372 of file CKalmanFilterCapable.h.

◆ OnInverseObservationModel() [3/3]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::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
inlinevirtualinherited

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_zThe observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
out_ynThe F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
out_dyn_dxvThe $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $.
out_dyn_dhnThe $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_dhnTSee 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 407 of file CKalmanFilterCapable.h.

◆ OnNewLandmarkAddedToMap()

void CRangeBearingKFSLAM2D::OnNewLandmarkAddedToMap ( const size_t  in_obsIdx,
const size_t  in_idxNewFeat 
)
protectedvirtual

If applicable to the given problem, do here any special handling of adding a new landmark to the map.

Parameters
in_obsIndexThe 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_idxNewFeatThe 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 >.

Definition at line 837 of file CRangeBearingKFSLAM2D.cpp.

References ASSERT_, ASSERTMSG_, mrpt::utils::bimap< KEY, VALUE >::insert(), m_IDs, m_last_data_association, m_SF, MRPT_END, MRPT_START, and mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::newly_inserted_landmarks.

◆ OnNormalizeStateVector()

void CRangeBearingKFSLAM2D::OnNormalizeStateVector ( )
protectedvirtual

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.

This virtual function musts normalize the state vector and covariance matrix (only if its necessary).

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

Definition at line 673 of file CRangeBearingKFSLAM2D.cpp.

References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, and mrpt::math::wrapToPiInPlace().

◆ OnObservationJacobians() [1/2]

void CRangeBearingKFSLAM2D::OnObservationJacobians ( const size_t &  idx_landmark_to_predict,
KFMatrix_OxV Hx,
KFMatrix_OxF Hy 
) const
protected

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

Parameters
idx_landmark_to_predictThe 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.
HxThe output Jacobian $ \frac{\partial h_i}{\partial x} $.
HyThe output Jacobian $ \frac{\partial h_i}{\partial y_i} $.

Definition at line 389 of file CRangeBearingKFSLAM2D.cpp.

References ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size(), m_SF, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, MRPT_START, and mrpt::math::square().

◆ OnObservationJacobians() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationJacobians ( const size_t &  idx_landmark_to_predict,
KFMatrix_OxV Hx,
KFMatrix_OxF Hy 
) const
inlineprotectedvirtualinherited

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

Parameters
idx_landmark_to_predictThe 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.
HxThe output Jacobian $ \frac{\partial h_i}{\partial x} $.
HyThe output Jacobian $ \frac{\partial h_i}{\partial y_i} $.

Definition at line 330 of file CKalmanFilterCapable.h.

◆ OnObservationJacobiansNumericGetIncrements() [1/2]

void CRangeBearingKFSLAM2D::OnObservationJacobiansNumericGetIncrements ( KFArray_VEH out_veh_increments,
KFArray_FEAT out_feat_increments 
) const
protected

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.

Definition at line 1080 of file CRangeBearingKFSLAM2D.cpp.

References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_feature_size(), and mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size().

◆ OnObservationJacobiansNumericGetIncrements() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationJacobiansNumericGetIncrements ( KFArray_VEH out_veh_increments,
KFArray_FEAT out_feat_increments 
) const
inlineprotectedvirtualinherited

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.

Definition at line 342 of file CKalmanFilterCapable.h.

◆ OnObservationModel() [1/2]

void CRangeBearingKFSLAM2D::OnObservationModel ( const vector_size_t idx_landmarks_to_predict,
vector_KFArray_OBS out_predictions 
) const
protected

◆ OnObservationModel() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationModel ( const mrpt::vector_size_t idx_landmarks_to_predict,
vector_KFArray_OBS out_predictions 
) const
protectedpure virtualinherited

Implements the observation prediction $ h_i(x) $.

Parameters
idx_landmark_to_predictThe 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_predictionsThe predicted observations.

◆ OnPostIteration()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnPostIteration ( )
inlinevirtualinherited

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

Definition at line 445 of file CKalmanFilterCapable.h.

◆ OnPreComputingPredictions() [1/2]

void CRangeBearingKFSLAM2D::OnPreComputingPredictions ( const vector_KFArray_OBS prediction_means,
vector_size_t out_LM_indices_to_predict 
) const
protected

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_meansThe 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_predictThe list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
Note
This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.
See also
OnGetObservations, OnDataAssociation

Definition at line 1044 of file CRangeBearingKFSLAM2D.cpp.

References ASSERTMSG_, DEG2RAD, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, m_SF, options, STATS_EXPERIMENT, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_range, and mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_yaw.

◆ OnPreComputingPredictions() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnPreComputingPredictions ( const vector_KFArray_OBS in_all_prediction_means,
mrpt::vector_size_t out_LM_indices_to_predict 
) const
inlineprotectedvirtualinherited

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_meansThe 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_predictThe list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
Note
This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.
See also
OnGetObservations, OnDataAssociation

Definition at line 279 of file CKalmanFilterCapable.h.

◆ OnSubstractObservationVectors() [1/2]

void CRangeBearingKFSLAM2D::OnSubstractObservationVectors ( KFArray_OBS A,
const KFArray_OBS B 
) const
protected

Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).

Definition at line 1020 of file CRangeBearingKFSLAM2D.cpp.

References mrpt::math::wrapToPiInPlace().

◆ OnSubstractObservationVectors() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnSubstractObservationVectors ( KFArray_OBS A,
const KFArray_OBS B 
) const
inlineprotectedvirtualinherited

Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).

Definition at line 352 of file CKalmanFilterCapable.h.

◆ OnTransitionJacobian() [1/2]

void CRangeBearingKFSLAM2D::OnTransitionJacobian ( KFMatrix_VxV out_F) const
protected

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

This virtual function musts calculate the Jacobian F of the prediction model.

Parameters
out_FMust 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).

Definition at line 243 of file CRangeBearingKFSLAM2D.cpp.

References m_action, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, MRPT_START, THROW_EXCEPTION, mrpt::math::TPoint2D::x, and mrpt::math::TPoint2D::y.

◆ OnTransitionJacobian() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionJacobian ( KFMatrix_VxV out_F) const
inlineprotectedvirtualinherited

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

Parameters
out_FMust 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).

Definition at line 253 of file CKalmanFilterCapable.h.

◆ OnTransitionJacobianNumericGetIncrements() [1/2]

void CRangeBearingKFSLAM2D::OnTransitionJacobianNumericGetIncrements ( KFArray_VEH out_increments) const
protected

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.

Definition at line 1072 of file CRangeBearingKFSLAM2D.cpp.

References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size().

◆ OnTransitionJacobianNumericGetIncrements() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionJacobianNumericGetIncrements ( KFArray_VEH out_increments) const
inlineprotectedvirtualinherited

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.

Definition at line 261 of file CKalmanFilterCapable.h.

◆ OnTransitionModel() [1/2]

void CRangeBearingKFSLAM2D::OnTransitionModel ( const KFArray_ACT in_u,
KFArray_VEH inout_x,
bool &  out_skipPrediction 
) const
protected

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

This virtual function musts implement the prediction model of the Kalman filter.

Parameters
in_uThe vector returned by OnGetAction.
inout_xAt input has

\[ \hat{x}_{k-1|k-1} \]

, at output must have $ \hat{x}_{k|k-1} $ .
out_skipSet 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

Definition at line 213 of file CRangeBearingKFSLAM2D.cpp.

References mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::get_vehicle_size(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, MRPT_START, mrpt::poses::CPose2D::phi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().

◆ OnTransitionModel() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionModel ( const KFArray_ACT in_u,
KFArray_VEH inout_x,
bool &  out_skipPrediction 
) const
protectedpure virtualinherited

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

Parameters
in_uThe vector returned by OnGetAction.
inout_xAt input has

\[ \hat{x}_{k-1|k-1} \]

, at output must have $ \hat{x}_{k|k-1} $ .
out_skipSet 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
Note
Even if you return "out_skip=true", the value of "inout_x" MUST be updated as usual (this is to allow numeric approximation of Jacobians).

◆ OnTransitionNoise() [1/2]

void CRangeBearingKFSLAM2D::OnTransitionNoise ( KFMatrix_VxV out_Q) const
protected

Implements the transition noise covariance $ Q_k $.

This virtual function musts calculate de noise matrix of the prediction model.

Parameters
out_QMust return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian

Definition at line 286 of file CRangeBearingKFSLAM2D.cpp.

References ASSERT_, mrpt::poses::CPosePDFGaussian::cov, m_action, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk, MRPT_END, MRPT_START, options, mrpt::poses::CPosePDFGaussian::rotateCov(), mrpt::math::square(), mrpt::slam::CRangeBearingKFSLAM2D::TOptions::stds_Q_no_odo, and THROW_EXCEPTION.

◆ OnTransitionNoise() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionNoise ( KFMatrix_VxV out_Q) const
protectedpure virtualinherited

Implements the transition noise covariance $ Q_k $.

Parameters
out_QMust return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian

◆ processActionObservation()

void CRangeBearingKFSLAM2D::processActionObservation ( mrpt::obs::CActionCollectionPtr &  action,
mrpt::obs::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
actionMay contain odometry
SFThe set of observations, must contain at least one CObservationBearingRange

Definition at line 149 of file CRangeBearingKFSLAM2D.cpp.

References ASSERT_, mrpt::slam::CRangeBearingKFSLAM2D::TOptions::create_simplemap, getCurrentRobotPose(), mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::getNumberOfLandmarksInTheMap(), mrpt::maps::CSimpleMap::insert(), m_action, m_IDs, m_SF, m_SFs, MRPT_END, MRPT_START, options, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::runOneKalmanIteration(), and mrpt::utils::bimap< KEY, VALUE >::size().

◆ reset()

void CRangeBearingKFSLAM2D::reset ( void  )

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

Definition at line 54 of file CRangeBearingKFSLAM2D.cpp.

References mrpt::utils::bimap< KEY, VALUE >::clear(), mrpt::maps::CSimpleMap::clear(), m_action, m_IDs, mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_pkk, m_SF, m_SFs, and mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::m_xkk.

Referenced by CRangeBearingKFSLAM2D().

◆ runOneKalmanIteration()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::runOneKalmanIteration ( )
protectedinherited

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 22 of file CKalmanFilterCapable_impl.h.

Referenced by processActionObservation().

◆ saveMapAndPath2DRepresentationAsMATLABFile()

void 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

Member Data Documentation

◆ KF_options

TKF_options mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KF_options
inherited

Generic options for the Kalman Filter algorithm itself.

Definition at line 462 of file CKalmanFilterCapable.h.

Referenced by loadOptions().

◆ m_action

mrpt::obs::CActionCollectionPtr mrpt::slam::CRangeBearingKFSLAM2D::m_action
protected

Set up by processActionObservation.

Definition at line 310 of file CRangeBearingKFSLAM2D.h.

Referenced by OnGetAction(), OnTransitionJacobian(), OnTransitionNoise(), processActionObservation(), and reset().

◆ m_IDs

mrpt::utils::bimap<mrpt::maps::CLandmark::TLandmarkID,unsigned int> mrpt::slam::CRangeBearingKFSLAM2D::m_IDs
protected

The mapping between landmark IDs and indexes in the Pkk cov.

matrix:

Definition at line 316 of file CRangeBearingKFSLAM2D.h.

Referenced by getCurrentState(), OnGetObservationsAndDataAssociation(), OnNewLandmarkAddedToMap(), processActionObservation(), and reset().

◆ m_last_data_association

TDataAssocInfo mrpt::slam::CRangeBearingKFSLAM2D::m_last_data_association
protected

Last data association.

Definition at line 321 of file CRangeBearingKFSLAM2D.h.

Referenced by OnGetObservationsAndDataAssociation(), and OnNewLandmarkAddedToMap().

◆ m_pkk

KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_pkk
protectedinherited

◆ m_SF

mrpt::obs::CSensoryFramePtr mrpt::slam::CRangeBearingKFSLAM2D::m_SF
protected

◆ m_SFs

mrpt::maps::CSimpleMap mrpt::slam::CRangeBearingKFSLAM2D::m_SFs
protected

The sequence of all the observations and the robot path (kept for debugging, statistics,etc)

Definition at line 319 of file CRangeBearingKFSLAM2D.h.

Referenced by processActionObservation(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().

◆ m_timLogger

mrpt::utils::CTimeLogger mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_timLogger
protectedinherited

Definition at line 226 of file CKalmanFilterCapable.h.

◆ m_xkk

KFVector mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_xkk
protectedinherited

◆ options

TOptions mrpt::slam::CRangeBearingKFSLAM2D::options



Page generated by Doxygen 1.8.14 for MRPT 1.5.8 Git: 81099f083 Sat May 4 08:22:57 2019 +0200 at sáb may 4 08:54:12 CEST 2019