Go to the documentation of this file.
9 #ifndef CKalmanFilterCapable_H
10 #define CKalmanFilterCapable_H
50 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
88 out <<
mrpt::format(
"\n----------- [TKF_options] ------------ \n\n");
93 "verbosity_level = %s\n",
100 "enable_profiler = %c\n",
135 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
141 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
144 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>&
obj);
147 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
153 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
156 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>&
obj);
159 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
164 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
165 const std::vector<int>& data_association,
167 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::KFMatrix_OxO&
R);
168 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
171 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>&
obj,
173 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
174 KFTYPE>::vector_KFArray_OBS& Z,
175 const std::vector<int>& data_association,
177 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
178 KFTYPE>::KFMatrix_OxO&
R);
221 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
222 typename KFTYPE =
double>
242 using KFVector = Eigen::Matrix<KFTYPE, Eigen::Dynamic, 1>;
284 &feat[0], &
m_xkk[VEH_SIZE + idx * FEAT_SIZE],
285 FEAT_SIZE *
sizeof(
m_xkk[0]));
294 VEH_SIZE + idx * FEAT_SIZE, VEH_SIZE + idx * FEAT_SIZE, feat_cov);
332 bool& out_skipPrediction)
const = 0;
353 for (
size_t i = 0; i < VEH_SIZE; i++) out_increments[i] = 1e-6;
380 std::vector<size_t>& out_LM_indices_to_predict)
const
385 out_LM_indices_to_predict.resize(N);
386 for (
size_t i = 0; i < N; i++) out_LM_indices_to_predict[i] = i;
427 const std::vector<size_t>& in_lm_indices_in_S,
438 const std::vector<size_t>& idx_landmarks_to_predict,
451 const size_t& idx_landmark_to_predict,
KFMatrix_OxV& Hx,
468 for (
size_t i = 0; i < VEH_SIZE; i++) out_veh_increments[i] = 1e-6;
469 for (
size_t i = 0; i < FEAT_SIZE; i++) out_feat_increments[i] = 1e-6;
514 "Inverse sensor model required but not implemented in derived "
561 bool& out_use_dyn_dhn_jacobian)
const
566 out_use_dyn_dhn_jacobian =
true;
581 const size_t in_obsIdx,
const size_t in_idxNewFeat)
652 const KFArray_VEH&
x,
const std::pair<KFCLASS*, KFArray_ACT>& dat,
655 const KFArray_VEH&
x,
const std::pair<KFCLASS*, size_t>& dat,
658 const KFArray_FEAT&
x,
const std::pair<KFCLASS*, size_t>& dat,
662 size_t VEH_SIZEb,
size_t OBS_SIZEb,
size_t FEAT_SIZEb,
size_t ACT_SIZEb,
666 VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb>&
obj,
668 VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb,
670 const std::vector<int>& data_association,
672 VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb>::
KFMatrix_OxO&
688 #include "CKalmanFilterCapable_impl.h"
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
virtual void OnPostIteration()
This method is called after finishing one KF iteration and before returning from runOneKalmanIteratio...
size_t getNumberOfLandmarksInTheMap() const
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.
static size_t get_vehicle_size()
#define MRPT_ENUM_TYPE_END()
#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...
TKFMethod method
The method to employ (default: kfEKFNaive)
static size_t get_observation_size()
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
static size_t get_feature_size()
TKF_options(mrpt::system::VerbosityLevel &verb_level_ref)
KFVector m_xkk
The system state vector.
mrpt::aligned_std_vector< KFMatrix_OxV > Hxs
The vector of all partial Jacobians dh[i]_dx for each prediction.
virtual void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
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...
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 th...
bool use_analytic_transition_jacobian
(default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimate...
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
bool enable_profiler
If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLo...
GLsizei GLsizei GLuint * obj
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
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...
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::COutputLogger COutputLogger
virtual void OnTransitionNoise(KFMatrix_VxV &out_Q) const =0
Implements the transition noise covariance .
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
MRPT_FILL_ENUM(kfEKFNaive)
virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &iniFile, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::aligned_std_vector< KFMatrix_OxF > Hys
The vector of all partial Jacobians dh[i]_dy[i] for each prediction.
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
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()
Eigen::Matrix< double, Eigen::Dynamic, 1 > KFVector
VerbosityLevel
Enumeration of available verbosity levels.
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 ...
This class allows loading and storing values and vectors of different types from a configuration text...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
bool m_user_didnt_implement_jacobian
mrpt::math::CMatrixTemplateNumeric< KFTYPE > KFMatrix
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 OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const =0
Implements the observation prediction .
string iniFile(myDataDir+string("benchmark-options.ini"))
std::vector< size_t > predictLMidxs
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.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
void dumpToTextStream(std::ostream &out) const override
This method must display clearly all the contents of the structure in textual form,...
double kftype
The numeric type used in the Kalman Filter (default=double)
A numeric matrix of compile-time fixed size.
KFVector & internal_getXkk()
Versatile class for consistent logging and management of output messages.
int IKF_iterations
Number of refinement iterations, only for the IKF method.
mrpt::system::VerbosityLevel & verbosity_level
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)
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
CKalmanFilterCapable()
Default constructor.
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 std::vector< int > &data_association, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO &R)
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.
GLsizei const GLchar ** string
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::system::CTimeLogger m_timLogger
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
mrpt::system::CTimeLogger & getProfiler()
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).
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.
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...
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |