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://www.mrpt.org/Kalman_Filters

The Kalman filter algorithms are generic, but this implementation is biased to ease the implementation of SLAM-like problems. However, it can be also applied to many generic problems not related to robotics or SLAM.

The meaning of the template parameters is:

VEH_SIZE: The dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
FEAT_SIZE: The dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).
KFTYPE: The numeric type of the matrices (default: double)

Revisions:

2007: Antonio J. Ortiz de Galisteo (AJOGD)
2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
2008/MAR: Implemented IKF (JLBC).
2009/DEC: Totally rewritten as a generic template using fixed-size matrices where possible (JLBC).

See also: mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D

Definition at line 52 of file CKalmanFilterCapable.h.

#include <mrpt/bayes/CKalmanFilterCapable.h>

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

Public Types
using	kftype = KFTYPE
	The numeric type used in the Kalman Filter (default=double) More...

using	KFCLASS = CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >
	My class, in a shorter name! More...

using	KFVector = Eigen::Matrix< KFTYPE, Eigen::Dynamic, 1 >

using	KFMatrix = mrpt::math::CMatrixTemplateNumeric< KFTYPE >

using	KFMatrix_VxV = mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, VEH_SIZE >

using	KFMatrix_OxO = mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, OBS_SIZE >

using	KFMatrix_FxF = mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, FEAT_SIZE >

using	KFMatrix_AxA = mrpt::math::CMatrixFixedNumeric< KFTYPE, ACT_SIZE, ACT_SIZE >

using	KFMatrix_VxO = mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, OBS_SIZE >

using	KFMatrix_VxF = mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, FEAT_SIZE >

using	KFMatrix_FxV = mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, VEH_SIZE >

using	KFMatrix_FxO = mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, OBS_SIZE >

using	KFMatrix_OxF = mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, FEAT_SIZE >

using	KFMatrix_OxV = mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, VEH_SIZE >

using	KFArray_VEH = mrpt::math::CArrayNumeric< KFTYPE, VEH_SIZE >

using	KFArray_ACT = mrpt::math::CArrayNumeric< KFTYPE, ACT_SIZE >

using	KFArray_OBS = mrpt::math::CArrayNumeric< KFTYPE, OBS_SIZE >

using	vector_KFArray_OBS = mrpt::aligned_std_vector< KFArray_OBS >

using	KFArray_FEAT = mrpt::math::CArrayNumeric< KFTYPE, FEAT_SIZE >

Public Member Functions
size_t	getNumberOfLandmarksInTheMap () const

bool	isMapEmpty () const

size_t	getStateVectorLength () const

KFVector &	internal_getXkk ()

KFMatrix &	internal_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...

	CKalmanFilterCapable ()
	Default constructor. More...

virtual	~CKalmanFilterCapable ()
	Destructor. More...

mrpt::system::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 ()

static std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > &	logging_levels_to_colors ()
	Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. More...

static std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > &	logging_levels_to_names ()
	Map from VerbosityLevels to their corresponding names. More...

Public Attributes
TKF_options	KF_options
	Generic options for the Kalman Filter algorithm itself. More...

Protected Member Functions
void	runOneKalmanIteration ()
	The main entry point, executes one complete step: prediction + update. More...

Protected Attributes
mrpt::system::CTimeLogger	m_timLogger

VerbosityLevel	m_min_verbosity_level
	Provided messages with VerbosityLevel smaller than this value shall be ignored. More...

Kalman filter state
KFVector	m_xkk
	The system state vector. More...

KFMatrix	m_pkk
	The system full covariance matrix. More...

Static Private Member Functions
static void	KF_aux_estimate_trans_jacobian (const KFArray_VEH &x, const std::pair< KFCLASS *, KFArray_ACT > &dat, KFArray_VEH &out_x)
	Auxiliary functions for Jacobian numeric estimation. More...

static void	KF_aux_estimate_obs_Hx_jacobian (const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)

static void	KF_aux_estimate_obs_Hy_jacobian (const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)

Private Attributes
vector_KFArray_OBS	all_predictions

std::vector< size_t >	predictLMidxs

mrpt::aligned_std_vector< KFMatrix_OxV >	Hxs
	The vector of all partial Jacobians dh[i]_dx for each prediction. More...

mrpt::aligned_std_vector< KFMatrix_OxF >	Hys
	The vector of all partial Jacobians dh[i]_dy[i] for each prediction. More...

KFMatrix	S

KFMatrix	Pkk_subset

vector_KFArray_OBS	Z

KFMatrix	K

KFMatrix	S_1

KFMatrix	dh_dx_full_obs

KFMatrix	aux_K_dh_dx

bool	m_user_didnt_implement_jacobian

Friends
template<size_t VEH_SIZEb, size_t OBS_SIZEb, size_t FEAT_SIZEb, size_t ACT_SIZEb, typename KFTYPEb >
void	detail::addNewLandmarks (CKalmanFilterCapable< VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb > &obj, const typename CKalmanFilterCapable< VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb >::vector_KFArray_OBS &Z, const std::vector< int > &data_association, const typename CKalmanFilterCapable< VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb >::KFMatrix_OxO &R)

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	OnNewLandmarkAddedToMap (const size_t in_obsIdx, const size_t in_idxNewFeat)
	If applicable to the given problem, do here any special handling of adding a new landmark to the map. More...

virtual void	OnNormalizeStateVector ()
	This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it. More...

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

virtual void	OnGetAction (KFArray_ACT &out_u) const =0
	Must return the action vector u. More...

virtual void	OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const =0
	Implements the transition model $\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 . More...

virtual void	OnPreComputingPredictions (const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const
	This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made. More...

virtual void	OnGetObservationNoise (KFMatrix_OxO &out_R) const =0
	Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. More...

virtual void	OnGetObservationsAndDataAssociation (vector_KFArray_OBS &out_z, std::vector< int > &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::vector< size_t > &in_lm_indices_in_S, const KFMatrix_OxO &in_R)=0
	This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map. More...

virtual void	OnObservationModel (const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const =0
	Implements the observation prediction . More...

virtual void	OnObservationJacobians (const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
	Implements the observation Jacobians $\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...

Logging methods
bool	logging_enable_console_output
	[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically. More...

bool	logging_enable_keep_record
	[Default=false] Enables storing all messages into an internal list. More...

void	logStr (const VerbosityLevel level, const std::string &msg_str) const
	Main method to add the specified message string to the logger. More...

void	logFmt (const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
	Alternative logging method, which mimics the printf behavior. More...

void void	logCond (const VerbosityLevel level, bool cond, const std::string &msg_str) const
	Log the given message only if the condition is satisfied. More...

void	setLoggerName (const std::string &name)
	Set the name of the COutputLogger instance. More...

std::string	getLoggerName () const
	Return the name of the COutputLogger instance. More...

void	setMinLoggingLevel (const VerbosityLevel level)
	Set the minimum logging level for which the incoming logs are going to be taken into account. More...

void	setVerbosityLevel (const VerbosityLevel level)
	alias of setMinLoggingLevel() More...

VerbosityLevel	getMinLoggingLevel () const

bool	isLoggingLevelVisible (VerbosityLevel level) const

void	getLogAsString (std::string &log_contents) const
	Fill the provided string with the contents of the logger's history in std::string representation. More...

std::string	getLogAsString () const
	Get the history of COutputLogger instance in a string representation. More...

void	writeLogToFile (const std::string *fname_in=NULL) const
	Write the contents of the COutputLogger instance to an external file. More...

void	dumpLogToConsole () const
	Dump the current contents of the COutputLogger instance in the terminal window. More...

std::string	getLoggerLastMsg () const
	Return the last Tmsg instance registered in the logger history. More...

void	getLoggerLastMsg (std::string &msg_str) const
	Fill inputtted string with the contents of the last message in history. More...

void	loggerReset ()
	Reset the contents of the logger instance. More...

void	logRegisterCallback (output_logger_callback_t userFunc)

bool	logDeregisterCallback (output_logger_callback_t userFunc)

Member Typedef Documentation

◆ KFArray_ACT

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_ACT = mrpt::math::CArrayNumeric<KFTYPE, ACT_SIZE>

Definition at line 268 of file CKalmanFilterCapable.h.

◆ KFArray_FEAT

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_FEAT = mrpt::math::CArrayNumeric<KFTYPE, FEAT_SIZE>

Definition at line 271 of file CKalmanFilterCapable.h.

◆ KFArray_OBS

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_OBS = mrpt::math::CArrayNumeric<KFTYPE, OBS_SIZE>

Definition at line 269 of file CKalmanFilterCapable.h.

◆ KFArray_VEH

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFArray_VEH = mrpt::math::CArrayNumeric<KFTYPE, VEH_SIZE>

Definition at line 267 of file CKalmanFilterCapable.h.

◆ KFCLASS

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFCLASS = CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>

My class, in a shorter name!

Definition at line 239 of file CKalmanFilterCapable.h.

◆ KFMatrix

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix = mrpt::math::CMatrixTemplateNumeric<KFTYPE>

Definition at line 243 of file CKalmanFilterCapable.h.

◆ KFMatrix_AxA

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_AxA = mrpt::math::CMatrixFixedNumeric<KFTYPE, ACT_SIZE, ACT_SIZE>

Definition at line 252 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxF

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_FxF = mrpt::math::CMatrixFixedNumeric<KFTYPE, FEAT_SIZE, FEAT_SIZE>

Definition at line 250 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxO

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_FxO = mrpt::math::CMatrixFixedNumeric<KFTYPE, FEAT_SIZE, OBS_SIZE>

Definition at line 261 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxV

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_FxV = mrpt::math::CMatrixFixedNumeric<KFTYPE, FEAT_SIZE, VEH_SIZE>

Definition at line 259 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxF

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxF = mrpt::math::CMatrixFixedNumeric<KFTYPE, OBS_SIZE, FEAT_SIZE>

Definition at line 263 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxO

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO = mrpt::math::CMatrixFixedNumeric<KFTYPE, OBS_SIZE, OBS_SIZE>

Definition at line 248 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxV

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxV = mrpt::math::CMatrixFixedNumeric<KFTYPE, OBS_SIZE, VEH_SIZE>

Definition at line 265 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxF

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_VxF = mrpt::math::CMatrixFixedNumeric<KFTYPE, VEH_SIZE, FEAT_SIZE>

Definition at line 257 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxO

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_VxO = mrpt::math::CMatrixFixedNumeric<KFTYPE, VEH_SIZE, OBS_SIZE>

Definition at line 255 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxV

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_VxV = mrpt::math::CMatrixFixedNumeric<KFTYPE, VEH_SIZE, VEH_SIZE>

Definition at line 246 of file CKalmanFilterCapable.h.

◆ kftype

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::kftype = KFTYPE

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

Definition at line 236 of file CKalmanFilterCapable.h.

◆ KFVector

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFVector = Eigen::Matrix<KFTYPE, Eigen::Dynamic, 1>

Definition at line 242 of file CKalmanFilterCapable.h.

◆ vector_KFArray_OBS

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

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::vector_KFArray_OBS = mrpt::aligned_std_vector<KFArray_OBS>

Definition at line 270 of file CKalmanFilterCapable.h.

Constructor & Destructor Documentation

◆ CKalmanFilterCapable()

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

◆ ~CKalmanFilterCapable()

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 ( )

inlinevirtual

Destructor.

Definition at line 614 of file CKalmanFilterCapable.h.

Member Function Documentation

◆ dumpLogToConsole()

void COutputLogger::dumpLogToConsole ( ) const

inherited

Dump the current contents of the COutputLogger instance in the terminal window.

See also: writeToFile

Definition at line 190 of file COutputLogger.cpp.

◆ get_action_size()

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 ( )

inlinestatic

Definition at line 229 of file CKalmanFilterCapable.h.

◆ get_feature_size()

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 ( )

inlinestatic

Definition at line 228 of file CKalmanFilterCapable.h.

◆ get_observation_size()

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 ( )

inlinestatic

Definition at line 227 of file CKalmanFilterCapable.h.

◆ get_vehicle_size()

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 ( )

inlinestatic

Definition at line 226 of file CKalmanFilterCapable.h.

◆ getLandmarkCov()

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 >::getLandmarkCov	(	size_t	idx,
		KFMatrix_FxF &	feat_cov
	)		const

inline

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

Exceptions

std::exception On idx>= getNumberOfLandmarksInTheMap()

Definition at line 291 of file CKalmanFilterCapable.h.

◆ getLandmarkMean()

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 >::getLandmarkMean	(	size_t	idx,
		KFArray_FEAT &	feat
	)		const

inline

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

Exceptions

std::exception On idx>= getNumberOfLandmarksInTheMap()

Definition at line 280 of file CKalmanFilterCapable.h.

◆ getLogAsString() [1/2]

void COutputLogger::getLogAsString ( std::string & log_contents ) const

inherited

Fill the provided string with the contents of the logger's history in std::string representation.

Definition at line 154 of file COutputLogger.cpp.

◆ getLogAsString() [2/2]

std::string COutputLogger::getLogAsString ( ) const

inherited

Get the history of COutputLogger instance in a string representation.

Definition at line 159 of file COutputLogger.cpp.

Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().

◆ getLoggerLastMsg() [1/2]

std::string COutputLogger::getLoggerLastMsg ( ) const

inherited

Return the last Tmsg instance registered in the logger history.

Definition at line 195 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::TMsg::getAsString().

◆ getLoggerLastMsg() [2/2]

void COutputLogger::getLoggerLastMsg ( std::string & msg_str ) const

inherited

Fill inputtted string with the contents of the last message in history.

Definition at line 201 of file COutputLogger.cpp.

◆ getLoggerName()

std::string COutputLogger::getLoggerName ( ) const

inherited

Return the name of the COutputLogger instance.

See also: setLoggerName

Definition at line 143 of file COutputLogger.cpp.

◆ getMinLoggingLevel()

VerbosityLevel mrpt::system::COutputLogger::getMinLoggingLevel ( ) const

inlineinherited

See also: setMinLoggingLevel

Definition at line 200 of file system/COutputLogger.h.

References mrpt::system::COutputLogger::m_min_verbosity_level.

Referenced by mrpt::maps::CRandomFieldGridMap2D::isEnabledVerbose(), and mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().

◆ getNumberOfLandmarksInTheMap()

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

Referenced by mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::getLandmarkMean(), and mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::OnPreComputingPredictions().

◆ getProfiler()

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

mrpt::system::CTimeLogger& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getProfiler ( )

inline

Definition at line 615 of file CKalmanFilterCapable.h.

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

◆ getStateVectorLength()

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 >::getStateVectorLength ( ) const

inline

Definition at line 273 of file CKalmanFilterCapable.h.

◆ internal_getPkk()

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 >::internal_getPkk ( )

inline

Definition at line 275 of file CKalmanFilterCapable.h.

◆ internal_getXkk()

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 >::internal_getXkk ( )

inline

Definition at line 274 of file CKalmanFilterCapable.h.

◆ isLoggingLevelVisible()

bool mrpt::system::COutputLogger::isLoggingLevelVisible ( VerbosityLevel level ) const

inlineinherited

Definition at line 201 of file system/COutputLogger.h.

References mrpt::system::COutputLogger::m_min_verbosity_level.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().

◆ isMapEmpty()

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

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

inline

Definition at line 234 of file CKalmanFilterCapable.h.

◆ KF_aux_estimate_obs_Hx_jacobian()

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

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KF_aux_estimate_obs_Hx_jacobian	(	const KFArray_VEH &	x,
		const std::pair< KFCLASS *, size_t > &	dat,
		KFArray_OBS &	out_x
	)

staticprivate

Definition at line 1217 of file CKalmanFilterCapable_impl.h.

◆ KF_aux_estimate_obs_Hy_jacobian()

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

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KF_aux_estimate_obs_Hy_jacobian	(	const KFArray_FEAT &	x,
		const std::pair< KFCLASS *, size_t > &	dat,
		KFArray_OBS &	out_x
	)

staticprivate

Definition at line 1234 of file CKalmanFilterCapable_impl.h.

◆ KF_aux_estimate_trans_jacobian()

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

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KF_aux_estimate_trans_jacobian	(	const KFArray_VEH &	x,
		const std::pair< KFCLASS *, KFArray_ACT > &	dat,
		KFArray_VEH &	out_x
	)

staticprivate

Auxiliary functions for Jacobian numeric estimation.

Definition at line 1204 of file CKalmanFilterCapable_impl.h.

◆ logCond()

void COutputLogger::logCond	(	const VerbosityLevel	level,
		bool	cond,
		const std::string &	msg_str
	)		const

inherited

Log the given message only if the condition is satisfied.

See also: log, logFmt

Definition at line 131 of file COutputLogger.cpp.

◆ logDeregisterCallback()

bool COutputLogger::logDeregisterCallback ( output_logger_callback_t userFunc )

inherited

Returns: true if an entry was found and deleted.

Definition at line 290 of file COutputLogger.cpp.

References getAddress(), and mrpt::system::COutputLogger::m_listCallbacks.

◆ logFmt()

void COutputLogger::logFmt	(	const VerbosityLevel	level,
		const char *	fmt,
			...
	)		const

inherited

Alternative logging method, which mimics the printf behavior.

Handy for not having to first use mrpt::format to pass a std::string message to logStr

// instead of:
logStr(mrpt::format("Today is the %d of %s, %d", 15, "July", 2016));
// one can use:
logFmt("Today is the %d of %s, %d", 15, "July", 2016);

See also: logStr, logCond

Definition at line 91 of file COutputLogger.cpp.

Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel(), CGraphSlamHandler< GRAPH_T >::execute(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), CGraphSlamHandler< GRAPH_T >::initOutputDir(), CGraphSlamHandler< GRAPH_T >::initVisualization(), mrpt::nav::CNavigatorManualSequence::navigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), CGraphSlamHandler< GRAPH_T >::readConfigFname(), CGraphSlamHandler< GRAPH_T >::saveResults(), CGraphSlamHandler< GRAPH_T >::setResultsDirName(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), and CGraphSlamHandler< GRAPH_T >::~CGraphSlamHandler().

◆ loggerReset()

void COutputLogger::loggerReset ( )

inherited

Reset the contents of the logger instance.

Called upon construction.

Definition at line 206 of file COutputLogger.cpp.

References mrpt::system::LVL_INFO.

◆ logging_levels_to_colors()

std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_colors ( )

staticinherited

Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor.

Handy for coloring the input based on the verbosity of the message

Definition at line 47 of file COutputLogger.cpp.

References logging_levels_to_colors.

Referenced by mrpt::system::COutputLogger::TMsg::dumpToConsole().

◆ logging_levels_to_names()

std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_names ( )

staticinherited

Map from VerbosityLevels to their corresponding names.

Handy for printing the current message VerbosityLevel along with the actual content

Definition at line 60 of file COutputLogger.cpp.

References logging_levels_to_names.

Referenced by mrpt::system::COutputLogger::TMsg::getAsString().

◆ logRegisterCallback()

void COutputLogger::logRegisterCallback ( output_logger_callback_t userFunc )

inherited

Definition at line 277 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::m_listCallbacks.

◆ logStr()

void COutputLogger::logStr	(	const VerbosityLevel	level,
		const std::string &	msg_str
	)		const

inherited

Main method to add the specified message string to the logger.

See also: logCond, logFmt

Definition at line 72 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::TMsg::body, mrpt::system::COutputLogger::TMsg::dumpToConsole(), mrpt::system::COutputLogger::TMsg::level, mrpt::system::COutputLogger::TMsg::name, and mrpt::system::COutputLogger::TMsg::timestamp.

Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().

◆ OnGetAction()

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 ) const

protectedpure virtual

Must return the action vector u.

Parameters

out_u The action vector which will be passed to OnTransitionModel

◆ OnGetObservationNoise()

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 ) const

protectedpure 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.

◆ OnGetObservationsAndDataAssociation()

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	(	vector_KFArray_OBS &	out_z,
		std::vector< int > &	out_data_association,
		const vector_KFArray_OBS &	in_all_predictions,
		const KFMatrix &	in_S,
		const std::vector< size_t > &	in_lm_indices_in_S,
		const KFMatrix_OxO &	in_R
	)

protectedpure 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 MO x MO 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.

◆ OnInverseObservationModel() [1/2]

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
	)		const

inlinevirtual

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.

Deprecated:: This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.

Definition at line 504 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::OnInverseObservationModel().

◆ OnInverseObservationModel() [2/2]

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,
		KFMatrix_FxF &	out_dyn_dhn_R_dyn_dhnT,
		bool &	out_use_dyn_dhn_jacobian
	)		const

inlinevirtual

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

◆ OnNewLandmarkAddedToMap()

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
	)

inlinevirtual

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

Parameters

in_obsIndex	The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.
in_idxNewFeat	The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.

See also: OnInverseObservationModel

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

Definition at line 580 of file CKalmanFilterCapable.h.

◆ OnNormalizeStateVector()

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 ( )

inlinevirtual

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

◆ OnObservationJacobians()

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
	)		const

inlineprotectedvirtual

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}$ .

Definition at line 450 of file CKalmanFilterCapable.h.

◆ OnObservationJacobiansNumericGetIncrements()

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 >::OnObservationJacobiansNumericGetIncrements	(	KFArray_VEH &	out_veh_increments,
		KFArray_FEAT &	out_feat_increments
	)		const

inlineprotectedvirtual

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

◆ OnObservationModel()

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 std::vector< size_t > &	idx_landmarks_to_predict,
		vector_KFArray_OBS &	out_predictions
	)		const

protectedpure 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.

◆ OnPostIteration()

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 ( )

inlinevirtual

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

Definition at line 600 of file CKalmanFilterCapable.h.

◆ OnPreComputingPredictions()

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,
		std::vector< size_t > &	out_LM_indices_to_predict
	)		const

inlineprotectedvirtual

This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.

For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.

Parameters

in_all_prediction_means	The mean of each landmark predictions; the computation or not of the corresponding covariances is what we're trying to determined with this method.
out_LM_indices_to_predict	The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.

Note: This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.

See also: OnGetObservations, OnDataAssociation

Definition at line 378 of file CKalmanFilterCapable.h.

◆ OnSubstractObservationVectors()

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
	)		const

inlineprotectedvirtual

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

◆ OnTransitionJacobian()

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 ) const

inlineprotectedvirtual

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

Definition at line 340 of file CKalmanFilterCapable.h.

◆ OnTransitionJacobianNumericGetIncrements()

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 >::OnTransitionJacobianNumericGetIncrements ( KFArray_VEH & out_increments ) const

inlineprotectedvirtual

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

◆ OnTransitionModel()

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
	)		const

protectedpure 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

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()

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 ) const

protectedpure 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

◆ runOneKalmanIteration()

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

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

protected

The main entry point, executes one complete step: prediction + update.

It is protected since derived classes must provide a problem-specific entry point for users. The exact order in which this method calls the virtual method is explained in http://www.mrpt.org/Kalman_Filters

Definition at line 27 of file CKalmanFilterCapable_impl.h.

◆ setLoggerName()

void COutputLogger::setLoggerName ( const std::string & name )

inherited

Set the name of the COutputLogger instance.

See also: getLoggerName

Definition at line 138 of file COutputLogger.cpp.

Referenced by mrpt::slam::CMetricMapBuilderICP::CMetricMapBuilderICP(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::slam::CMonteCarloLocalization2D::CMonteCarloLocalization2D(), mrpt::slam::CMonteCarloLocalization3D::CMonteCarloLocalization3D(), and mrpt::graphslam::CWindowManager::initCWindowManager().

◆ setMinLoggingLevel()

void COutputLogger::setMinLoggingLevel ( const VerbosityLevel level )

inherited

Set the minimum logging level for which the incoming logs are going to be taken into account.

String messages with specified VerbosityLevel smaller than the min, will not be outputted to the screen and neither will a record of them be stored in by the COutputLogger instance

Definition at line 144 of file COutputLogger.cpp.

Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), mrpt::hwdrivers::CHokuyoURG::initialize(), and mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams().

◆ setVerbosityLevel()

void COutputLogger::setVerbosityLevel ( const VerbosityLevel level )

inherited

alias of setMinLoggingLevel()

Definition at line 149 of file COutputLogger.cpp.

Referenced by mrpt::nav::CAbstractNavigator::CAbstractNavigator(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::comms::CServerTCPSocket::CServerTCPSocket(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::math::ransac_detect_2D_lines().

◆ writeLogToFile()

void COutputLogger::writeLogToFile ( const std::string * fname_in = NULL ) const

inherited

Write the contents of the COutputLogger instance to an external file.

Upon call to this method, COutputLogger dumps the contents of all the logged commands so far to the specified external file. By default the filename is set to ${LOGGERNAME}.log except if the fname parameter is provided

See also: dumpToConsole, getAsString

Definition at line 165 of file COutputLogger.cpp.

References ASSERTMSG_, and mrpt::format().

Friends And Related Function Documentation

◆ detail::addNewLandmarks

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

template<size_t VEH_SIZEb, size_t OBS_SIZEb, size_t FEAT_SIZEb, size_t ACT_SIZEb, typename KFTYPEb >

void detail::addNewLandmarks	(	CKalmanFilterCapable< VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb > &	obj,
		const typename CKalmanFilterCapable< VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb >::vector_KFArray_OBS &	Z,
		const std::vector< int > &	data_association,
		const typename CKalmanFilterCapable< VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb >::KFMatrix_OxO &	R
	)

friend

Member Data Documentation

◆ all_predictions

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

vector_KFArray_OBS mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::all_predictions

private

Definition at line 623 of file CKalmanFilterCapable.h.

◆ aux_K_dh_dx

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 >::aux_K_dh_dx

private

Definition at line 635 of file CKalmanFilterCapable.h.

◆ dh_dx_full_obs

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 >::dh_dx_full_obs

private

Definition at line 634 of file CKalmanFilterCapable.h.

◆ Hxs

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

mrpt::aligned_std_vector<KFMatrix_OxV> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::Hxs

private

The vector of all partial Jacobians dh[i]_dx for each prediction.

Definition at line 626 of file CKalmanFilterCapable.h.

◆ Hys

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

mrpt::aligned_std_vector<KFMatrix_OxF> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::Hys

private

The vector of all partial Jacobians dh[i]_dy[i] for each prediction.

Definition at line 628 of file CKalmanFilterCapable.h.

◆ K

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 >::K

private

Definition at line 632 of file CKalmanFilterCapable.h.

◆ KF_options

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

◆ logging_enable_console_output

bool mrpt::system::COutputLogger::logging_enable_console_output

inherited

[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically.

Definition at line 239 of file system/COutputLogger.h.

◆ logging_enable_keep_record

bool mrpt::system::COutputLogger::logging_enable_keep_record

inherited

[Default=false] Enables storing all messages into an internal list.

See also: writeLogToFile, getLogAsString

Definition at line 242 of file system/COutputLogger.h.

◆ m_min_verbosity_level

VerbosityLevel mrpt::system::COutputLogger::m_min_verbosity_level

protectedinherited

Provided messages with VerbosityLevel smaller than this value shall be ignored.

Definition at line 252 of file system/COutputLogger.h.

Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().

◆ m_pkk

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

Referenced by mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::getLandmarkCov(), and mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::internal_getPkk().

◆ m_timLogger

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

mrpt::system::CTimeLogger mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_timLogger

protected

Definition at line 308 of file CKalmanFilterCapable.h.

◆ m_user_didnt_implement_jacobian

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

bool mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_user_didnt_implement_jacobian

mutableprivate

Definition at line 647 of file CKalmanFilterCapable.h.

Referenced by mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::OnObservationJacobians(), and mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::OnTransitionJacobian().

◆ m_xkk

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

Referenced by mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::getLandmarkMean(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::getStateVectorLength(), and mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::internal_getXkk().

◆ Pkk_subset

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 >::Pkk_subset

private

Definition at line 630 of file CKalmanFilterCapable.h.

◆ predictLMidxs

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

std::vector<size_t> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::predictLMidxs

private

Definition at line 624 of file CKalmanFilterCapable.h.

◆ S

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 >::S

private

Definition at line 629 of file CKalmanFilterCapable.h.

◆ S_1

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 >::S_1

private

Definition at line 633 of file CKalmanFilterCapable.h.

◆ Z

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

vector_KFArray_OBS mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::Z

private

Definition at line 631 of file CKalmanFilterCapable.h.

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 >

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Static Private Member Functions

Private Attributes

Friends

Virtual methods for Kalman Filter implementation

Logging methods

Member Typedef Documentation

◆ KFArray_ACT

◆ KFArray_FEAT

◆ KFArray_OBS

◆ KFArray_VEH

◆ KFCLASS

◆ KFMatrix

◆ KFMatrix_AxA

◆ KFMatrix_FxF

◆ KFMatrix_FxO

◆ KFMatrix_FxV

◆ KFMatrix_OxF

◆ KFMatrix_OxO

◆ KFMatrix_OxV

◆ KFMatrix_VxF

◆ KFMatrix_VxO

◆ KFMatrix_VxV

◆ kftype

◆ KFVector

◆ vector_KFArray_OBS

Constructor & Destructor Documentation

◆ CKalmanFilterCapable()

◆ ~CKalmanFilterCapable()

Member Function Documentation

◆ dumpLogToConsole()

◆ get_action_size()

◆ get_feature_size()

◆ get_observation_size()

◆ get_vehicle_size()

◆ getLandmarkCov()

◆ getLandmarkMean()

◆ getLogAsString() [1/2]

◆ getLogAsString() [2/2]

◆ getLoggerLastMsg() [1/2]

◆ getLoggerLastMsg() [2/2]

◆ getLoggerName()

◆ getMinLoggingLevel()

◆ getNumberOfLandmarksInTheMap()

◆ getProfiler()

◆ getStateVectorLength()

◆ internal_getPkk()

◆ internal_getXkk()

◆ isLoggingLevelVisible()

◆ isMapEmpty()

◆ KF_aux_estimate_obs_Hx_jacobian()

◆ KF_aux_estimate_obs_Hy_jacobian()

◆ KF_aux_estimate_trans_jacobian()

◆ logCond()

◆ logDeregisterCallback()

◆ logFmt()

◆ loggerReset()

◆ logging_levels_to_colors()

◆ logging_levels_to_names()

◆ logRegisterCallback()

◆ logStr()

◆ OnGetAction()

◆ OnGetObservationNoise()

◆ OnGetObservationsAndDataAssociation()

◆ OnInverseObservationModel() [1/2]

◆ OnInverseObservationModel() [2/2]

◆ OnNewLandmarkAddedToMap()

◆ OnNormalizeStateVector()

◆ OnObservationJacobians()

◆ OnObservationJacobiansNumericGetIncrements()

◆ OnObservationModel()

◆ OnPostIteration()

◆ OnPreComputingPredictions()

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 >