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;
69 const std::string §ion)
84 out.
printf(
"\n----------- [TKF_options] ------------ \n\n");
107 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
110 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
113 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
116 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
119 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
125 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
161 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE =
double>
177 typedef Eigen::Matrix<KFTYPE,Eigen::Dynamic,1>
KFVector;
209 ASSERT_(idx<getNumberOfLandmarksInTheMap())
210 ::
memcpy(&feat[0], &m_xkk[VEH_SIZE+idx*FEAT_SIZE], FEAT_SIZE*
sizeof(m_xkk[0]));
216 m_pkk.extractMatrix(VEH_SIZE+idx*FEAT_SIZE,VEH_SIZE+idx*FEAT_SIZE,feat_cov);
237 virtual void OnGetAction( KFArray_ACT &out_u )
const = 0;
245 virtual void OnTransitionModel(
246 const KFArray_ACT &in_u,
247 KFArray_VEH &inout_x,
248 bool &out_skipPrediction
258 m_user_didnt_implement_jacobian=
true;
265 for (
size_t i=0;i<VEH_SIZE;i++) out_increments[i] = 1e-6;
272 virtual void OnTransitionNoise( KFMatrix_VxV &out_Q )
const = 0;
282 const vector_KFArray_OBS &in_all_prediction_means,
287 const size_t N = this->getNumberOfLandmarksInTheMap();
288 out_LM_indices_to_predict.resize(N);
289 for (
size_t i=0;i<N;i++) out_LM_indices_to_predict[i]=i;
296 virtual void OnGetObservationNoise(KFMatrix_OxO &out_R)
const = 0;
309 virtual void OnGetObservationsAndDataAssociation(
310 vector_KFArray_OBS &out_z,
312 const vector_KFArray_OBS &in_all_predictions,
313 const KFMatrix &in_S,
315 const KFMatrix_OxO &in_R
322 virtual void OnObservationModel(
324 vector_KFArray_OBS &out_predictions
333 const size_t &idx_landmark_to_predict,
339 m_user_didnt_implement_jacobian=
true;
345 KFArray_VEH &out_veh_increments,
346 KFArray_FEAT &out_feat_increments )
const 348 for (
size_t i=0;i<VEH_SIZE;i++) out_veh_increments[i] = 1e-6;
349 for (
size_t i=0;i<FEAT_SIZE;i++) out_feat_increments[i] = 1e-6;
375 const KFArray_OBS & in_z,
376 KFArray_FEAT & out_yn,
377 KFMatrix_FxV & out_dyn_dxv,
378 KFMatrix_FxO & out_dyn_dhn )
const 383 THROW_EXCEPTION(
"Inverse sensor model required but not implemented in derived class.")
410 const KFArray_OBS & in_z,
411 KFArray_FEAT & out_yn,
412 KFMatrix_FxV & out_dyn_dxv,
413 KFMatrix_FxO & out_dyn_dhn,
414 KFMatrix_FxF & out_dyn_dhn_R_dyn_dhnT,
415 bool & out_use_dyn_dhn_jacobian
420 OnInverseObservationModel(in_z,out_yn,out_dyn_dxv,out_dyn_dhn);
421 out_use_dyn_dhn_jacobian=
true;
431 const size_t in_obsIdx,
432 const size_t in_idxNewFeat )
468 vector_KFArray_OBS all_predictions;
474 vector_KFArray_OBS Z;
477 KFMatrix dh_dx_full_obs;
478 KFMatrix aux_K_dh_dx;
486 void runOneKalmanIteration();
489 mutable bool m_user_didnt_implement_jacobian;
492 static void KF_aux_estimate_trans_jacobian(
const KFArray_VEH &x,
const std::pair<KFCLASS*,KFArray_ACT> &dat, KFArray_VEH &out_x);
493 static void KF_aux_estimate_obs_Hx_jacobian(
const KFArray_VEH &x,
const std::pair<KFCLASS*,size_t> &dat, KFArray_OBS &out_x);
494 static void KF_aux_estimate_obs_Hy_jacobian(
const KFArray_FEAT &x,
const std::pair<KFCLASS*,size_t> &dat,KFArray_OBS &out_x);
496 template <
size_t VEH_SIZEb,
size_t OBS_SIZEb,
size_t FEAT_SIZEb,
size_t ACT_SIZEb,
typename KFTYPEb>
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, FEAT_SIZE > KFMatrix_FxF
mrpt::math::CArrayNumeric< KFTYPE, FEAT_SIZE > KFArray_FEAT
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...
static size_t get_action_size()
bool use_analytic_observation_jacobian
(default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estima...
mrpt::math::CArrayNumeric< KFTYPE, OBS_SIZE > KFArray_OBS
virtual void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
static void fill(bimap< enum_t, std::string > &m_map)
ENUMTYPE read_enum(const std::string §ion, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
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...
#define THROW_EXCEPTION(msg)
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
double debug_verify_analytic_jacobians_threshold
(default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians...
Only specializations of this class are defined for each enum type of interest.
KFMatrix m_pkk
The system full covariance matrix.
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...
size_t getNumberOfLandmarksInTheMap() const
virtual void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::utils::CTimeLogger m_timLogger
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > KFCLASS
My class, in a shorter name!
A numeric matrix of compile-time fixed size.
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
static size_t get_vehicle_size()
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
mrpt::math::CArrayNumeric< KFTYPE, VEH_SIZE > KFArray_VEH
A helper class that can convert an enum value into its textual representation, and viceversa...
bool verbose
If set to true timing and other information will be dumped during the execution (default=false) ...
static size_t get_observation_size()
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
mrpt::math::CArrayNumeric< KFTYPE, ACT_SIZE > KFArray_ACT
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
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 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...
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)
KFVector & internal_getXkk()
Generic options for the Kalman Filter algorithm in itself.
void dumpToTextStream(mrpt::utils::CStream &out) const
This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
KFTYPE kftype
The numeric type used in the Kalman Filter (default=double)
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...
#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...
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
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...
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, FEAT_SIZE > KFMatrix_VxF
mrpt::math::CMatrixFixedNumeric< KFTYPE, ACT_SIZE, ACT_SIZE > KFMatrix_AxA
mrpt::math::CMatrixTemplateNumeric< KFTYPE > KFMatrix
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE > &obj)
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &iniFile, const std::string §ion)
This method load the options from a ".ini"-like file or memory-stored string list.
bool debug_verify_analytic_jacobians
(default=false) If true, will compute all the Jacobians numerically and compare them to the analytica...
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
TKFMethod
The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, OBS_SIZE > KFMatrix_VxO
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
std::vector< size_t > vector_size_t
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...
std::vector< int32_t > vector_int
TKFMethod method
The method to employ (default: kfEKFNaive)
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE > &obj)
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
KFMatrix & internal_getPkk()
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
size_t getStateVectorLength() const
static size_t get_feature_size()
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)...
KFVector m_xkk
The system state vector.
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
This base class provides a common printf-like method to send debug information to std::cout...
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
bool use_analytic_transition_jacobian
(default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimate...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void addNewLandmarks(CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE > &obj, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE >::vector_KFArray_OBS &Z, const vector_int &data_association, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE >::KFMatrix_OxO &R)
int IKF_iterations
Number of refinement iterations, only for the IKF method.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
virtual void OnPostIteration()
This method is called after finishing one KF iteration and before returning from runOneKalmanIteratio...
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
Eigen::Matrix< KFTYPE, Eigen::Dynamic, 1 > KFVector
std::vector< int32_t > vector_int
std::vector< size_t > vector_size_t