Go to the documentation of this file.
9 #ifndef CKalmanFilterCapable_H
10 #define CKalmanFilterCapable_H
48 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
class CKalmanFilterCapable;
82 out.printf(
"\n----------- [TKF_options] ------------ \n\n");
84 out.printf(
"verbose = %c\n",
verbose ?
'Y':
'N');
105 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
108 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
111 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
114 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
117 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
123 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
159 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE =
double>
175 typedef Eigen::Matrix<KFTYPE,Eigen::Dynamic,1>
KFVector;
208 ::
memcpy(&feat[0], &
m_xkk[VEH_SIZE+idx*FEAT_SIZE], FEAT_SIZE*
sizeof(
m_xkk[0]));
214 m_pkk.extractMatrix(VEH_SIZE+idx*FEAT_SIZE,VEH_SIZE+idx*FEAT_SIZE,feat_cov);
246 bool &out_skipPrediction
263 for (
size_t i=0;i<VEH_SIZE;i++) out_increments[i] = 1e-6;
286 out_LM_indices_to_predict.resize(N);
287 for (
size_t i=0;i<N;i++) out_LM_indices_to_predict[i]=i;
331 const size_t &idx_landmark_to_predict,
346 for (
size_t i=0;i<VEH_SIZE;i++) out_veh_increments[i] = 1e-6;
347 for (
size_t i=0;i<FEAT_SIZE;i++) out_feat_increments[i] = 1e-6;
381 THROW_EXCEPTION(
"Inverse sensor model required but not implemented in derived class.")
413 bool & out_use_dyn_dhn_jacobian
419 out_use_dyn_dhn_jacobian=
true;
429 const size_t in_obsIdx,
430 const size_t in_idxNewFeat )
494 template <
size_t VEH_SIZEb,
size_t OBS_SIZEb,
size_t FEAT_SIZEb,
size_t ACT_SIZEb,
typename KFTYPEb>
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
#define THROW_EXCEPTION(msg)
virtual void OnPostIteration()
This method is called after finishing one KF iteration and before returning from runOneKalmanIteratio...
size_t getNumberOfLandmarksInTheMap() const
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
void addNewLandmarks(CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::vector_KFArray_OBS &Z, const vector_int &data_association, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO &R)
mrpt::math::CArrayNumeric< KFTYPE, FEAT_SIZE > KFArray_FEAT
This base class provides a common printf-like method to send debug information to std::cout,...
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.
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...
static size_t get_vehicle_size()
mrpt::math::CMatrixFixedNumeric< KFTYPE, ACT_SIZE, ACT_SIZE > KFMatrix_AxA
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, OBS_SIZE > KFMatrix_VxO
virtual void OnObservationModel(const mrpt::vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const =0
Implements the observation prediction .
TKFMethod method
The method to employ (default: kfEKFNaive)
static size_t get_observation_size()
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
static size_t get_feature_size()
KFVector m_xkk
The system state vector.
static void fill(bimap< enum_t, std::string > &m_map)
virtual void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
mrpt::math::CMatrixTemplateNumeric< KFTYPE > KFMatrix
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 ex...
CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > KFCLASS
My class, in a shorter name!
bool use_analytic_transition_jacobian
(default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimate...
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
bool enable_profiler
If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLo...
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
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 th...
virtual void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
mrpt::aligned_containers< KFMatrix_OxF >::vector_t Hys
The vector of all partial Jacobians dh[i]_dy[i] for each prediction.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void OnTransitionNoise(KFMatrix_VxV &out_Q) const =0
Implements the transition noise covariance .
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, FEAT_SIZE > KFMatrix_VxF
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &iniFile, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
mrpt::math::CArrayNumeric< KFTYPE, OBS_SIZE > KFArray_OBS
vector_KFArray_OBS all_predictions
Generic options for the Kalman Filter algorithm in itself.
bool debug_verify_analytic_jacobians
(default=false) If true, will compute all the Jacobians numerically and compare them to the analytica...
KFMatrix m_pkk
The system full covariance matrix.
static size_t get_action_size()
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
size_t getStateVectorLength() const
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 ...
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
mrpt::math::CArrayNumeric< KFTYPE, ACT_SIZE > KFArray_ACT
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
bool m_user_didnt_implement_jacobian
TKFMethod
The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...
virtual void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
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.
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
vector_size_t predictLMidxs
mrpt::utils::CTimeLogger & getProfiler()
A numeric matrix of compile-time fixed size.
KFVector & internal_getXkk()
int IKF_iterations
Number of refinement iterations, only for the IKF method.
mrpt::math::CArrayNumeric< KFTYPE, VEH_SIZE > KFArray_VEH
std::vector< int32_t > vector_int
KFMatrix & internal_getPkk()
double debug_verify_analytic_jacobians_threshold
(default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
mrpt::aligned_containers< KFMatrix_OxV >::vector_t Hxs
The vector of all partial Jacobians dh[i]_dx for each prediction.
mrpt::utils::CTimeLogger m_timLogger
KFTYPE kftype
The numeric type used in the Kalman Filter (default=double)
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method must display clearly all the contents of the structure in textual form,...
CKalmanFilterCapable()
Default constructor.
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
virtual ~CKalmanFilterCapable()
Destructor.
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, FEAT_SIZE > KFMatrix_FxF
virtual void OnGetAction(KFArray_ACT &out_u) const =0
Must return the action vector u.
virtual void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const =0
Implements the transition model .
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
std::vector< size_t > vector_size_t
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).
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
bool verbose
If set to true timing and other information will be dumped during the execution (default=false)
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 scala...
void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov) const
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
A helper class that can convert an enum value into its textual representation, and viceversa.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Only specializations of this class are defined for each enum type of interest.
Eigen::Matrix< KFTYPE, Eigen::Dynamic, 1 > KFVector
bool use_analytic_observation_jacobian
(default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estima...
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 ex...
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 th...
Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 23:08:25 UTC 2019 | | |