Main MRPT website > C++ reference for MRPT 1.4.0
CKalmanFilterCapable.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef CKalmanFilterCapable_H
10#define CKalmanFilterCapable_H
11
16#include <mrpt/math/utils.h>
24#include <mrpt/utils/stl_containers_utils.h> // find_in_vector
25#include <mrpt/utils/CTicTac.h>
29
30
31namespace mrpt
32{
33 namespace bayes
34 {
35 /** The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable
36 * For further details on each algorithm see the tutorial: http://www.mrpt.org/Kalman_Filters
37 * \sa bayes::CKalmanFilterCapable::KF_options
38 * \ingroup mrpt_bayes_grp
39 */
40 enum TKFMethod {
44 kfIKF
45 };
46
47 // Forward declaration:
48 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE> class CKalmanFilterCapable;
49
50 /** Generic options for the Kalman Filter algorithm in itself.
51 * \ingroup mrpt_bayes_grp
52 */
54 {
57 verbose ( false ),
58 IKF_iterations ( 5 ),
59 enable_profiler (false),
64 {
65 }
66
67 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &iniFile,const std::string &section) MRPT_OVERRIDE
68 {
69 method = iniFile.read_enum<TKFMethod>(section,"method", method );
70 MRPT_LOAD_CONFIG_VAR( verbose, bool , iniFile, section );
71 MRPT_LOAD_CONFIG_VAR( IKF_iterations, int , iniFile, section );
72 MRPT_LOAD_CONFIG_VAR( enable_profiler, bool , iniFile, section );
75 MRPT_LOAD_CONFIG_VAR( debug_verify_analytic_jacobians, bool , iniFile, section );
77 }
78
79 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. */
81 {
82 out.printf("\n----------- [TKF_options] ------------ \n\n");
83 out.printf("method = %s\n", mrpt::utils::TEnumType<TKFMethod>::value2name(method).c_str() );
84 out.printf("verbose = %c\n", verbose ? 'Y':'N');
85 out.printf("IKF_iterations = %i\n", IKF_iterations);
86 out.printf("enable_profiler = %c\n", enable_profiler ? 'Y':'N');
87 out.printf("\n");
88 }
89
90
91 TKFMethod method; //!< The method to employ (default: kfEKFNaive)
92 bool verbose; //!< If set to true timing and other information will be dumped during the execution (default=false)
93 int IKF_iterations; //!< Number of refinement iterations, only for the IKF method.
94 bool enable_profiler;//!< If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLog at the end of the execution.
95 bool use_analytic_transition_jacobian; //!< (default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimated from a numeric approximation by calling several times to OnTransitionModel.
96 bool use_analytic_observation_jacobian; //!< (default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estimated from a numeric approximation by calling several times to OnObservationModel.
97 bool debug_verify_analytic_jacobians; //!< (default=false) If true, will compute all the Jacobians numerically and compare them to the analytical ones, throwing an exception on mismatch.
98 double debug_verify_analytic_jacobians_threshold; //!< (default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians
99 };
100
101 /** Auxiliary functions, for internal usage of MRPT classes */
102 namespace detail
103 {
104 // Auxiliary functions.
105 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
107 // Specialization:
108 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
110
111 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
113 // Specialization:
114 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
116
117 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
118 void addNewLandmarks(
121 const vector_int &data_association,
123 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
124 void addNewLandmarks(
127 const vector_int &data_association,
129 }
130
131
132 /** Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
133 * This base class stores the state vector and covariance matrix of the system. It has virtual methods that must be completed
134 * by derived classes to address a given filtering problem. The main entry point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which
135 * should be called AFTER setting the desired filter options in KF_options, as well as any options in the derived class.
136 * 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.
137 *
138 * For further details and examples, check out the tutorial: http://www.mrpt.org/Kalman_Filters
139 *
140 * The Kalman filter algorithms are generic, but this implementation is biased to ease the implementation
141 * of SLAM-like problems. However, it can be also applied to many generic problems not related to robotics or SLAM.
142 *
143 * The meaning of the template parameters is:
144 * - VEH_SIZE: The dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
145 * - OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
146 * - FEAT_SIZE: The dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
147 * - ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).
148 * - KFTYPE: The numeric type of the matrices (default: double)
149 *
150 * Revisions:
151 * - 2007: Antonio J. Ortiz de Galisteo (AJOGD)
152 * - 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
153 * - 2008/MAR: Implemented IKF (JLBC).
154 * - 2009/DEC: Totally rewritten as a generic template using fixed-size matrices where possible (JLBC).
155 *
156 * \sa mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D
157 * \ingroup mrpt_bayes_grp
158 */
159 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
161 {
162 public:
163 static inline size_t get_vehicle_size() { return VEH_SIZE; }
164 static inline size_t get_observation_size() { return OBS_SIZE; }
165 static inline size_t get_feature_size() { return FEAT_SIZE; }
166 static inline size_t get_action_size() { return ACT_SIZE; }
168 inline bool isMapEmpty() const { return detail::isMapEmpty(*this); }
169
170
171 typedef KFTYPE kftype; //!< The numeric type used in the Kalman Filter (default=double)
173
174 // ---------- Many useful typedefs to short the notation a bit... --------
175 typedef Eigen::Matrix<KFTYPE,Eigen::Dynamic,1> KFVector;
177
182
185
188
191
197
198 inline size_t getStateVectorLength() const { return m_xkk.size(); }
199
200 inline KFVector& internal_getXkk() { return m_xkk; }
201 inline KFMatrix& internal_getPkk() { return m_pkk; }
202
203 /** Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).
204 * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
205 */
206 inline void getLandmarkMean(size_t idx, KFArray_FEAT &feat ) const {
208 ::memcpy(&feat[0], &m_xkk[VEH_SIZE+idx*FEAT_SIZE], FEAT_SIZE*sizeof(m_xkk[0]));
209 }
210 /** Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
211 * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
212 */
213 inline void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov ) const {
214 m_pkk.extractMatrix(VEH_SIZE+idx*FEAT_SIZE,VEH_SIZE+idx*FEAT_SIZE,feat_cov);
215 }
216
217 protected:
218 /** @name Kalman filter state
219 @{ */
220
221 KFVector m_xkk; //!< The system state vector.
222 KFMatrix m_pkk; //!< The system full covariance matrix.
223
224 /** @} */
225
227
228 /** @name Virtual methods for Kalman Filter implementation
229 @{
230 */
231
232 /** Must return the action vector u.
233 * \param out_u The action vector which will be passed to OnTransitionModel
234 */
235 virtual void OnGetAction( KFArray_ACT &out_u ) const = 0;
236
237 /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
238 * \param in_u The vector returned by OnGetAction.
239 * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must have \f$ \hat{x}_{k|k-1} \f$ .
240 * \param 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
241 * \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).
242 */
243 virtual void OnTransitionModel(
244 const KFArray_ACT &in_u,
245 KFArray_VEH &inout_x,
246 bool &out_skipPrediction
247 ) const = 0;
248
249 /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
250 * \param out_F Must return the Jacobian.
251 * The returned matrix must be \f$V \times V\f$ with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).
252 */
253 virtual void OnTransitionJacobian( KFMatrix_VxV &out_F ) const
254 {
255 MRPT_UNUSED_PARAM(out_F);
257 }
258
259 /** 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.
260 */
261 virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
262 {
263 for (size_t i=0;i<VEH_SIZE;i++) out_increments[i] = 1e-6;
264 }
265
266 /** Implements the transition noise covariance \f$ Q_k \f$
267 * \param out_Q Must return the covariance matrix.
268 * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
269 */
270 virtual void OnTransitionNoise( KFMatrix_VxV &out_Q ) const = 0;
271
272 /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
273 * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
274 * \param 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.
275 * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
276 * \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.
277 * \sa OnGetObservations, OnDataAssociation
278 */
280 const vector_KFArray_OBS &in_all_prediction_means,
281 mrpt::vector_size_t &out_LM_indices_to_predict ) const
282 {
283 MRPT_UNUSED_PARAM(in_all_prediction_means);
284 // Default: all of them:
285 const size_t N = this->getNumberOfLandmarksInTheMap();
286 out_LM_indices_to_predict.resize(N);
287 for (size_t i=0;i<N;i++) out_LM_indices_to_predict[i]=i;
288 }
289
290 /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
291 * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
292 * \note Upon call, it can be assumed that the previous contents of out_R are all zeros.
293 */
294 virtual void OnGetObservationNoise(KFMatrix_OxO &out_R) const = 0;
295
296 /** 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.
297 *
298 * \param 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.
299 * \param 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.
300 * \param 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.
301 * \param in_S The full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a M*O x M*O matrix with M=length of "in_lm_indices_in_S".
302 * \param in_lm_indices_in_S The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
303 *
304 * This method will be called just once for each complete KF iteration.
305 * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
306 */
308 vector_KFArray_OBS &out_z,
309 mrpt::vector_int &out_data_association,
310 const vector_KFArray_OBS &in_all_predictions,
311 const KFMatrix &in_S,
312 const vector_size_t &in_lm_indices_in_S,
313 const KFMatrix_OxO &in_R
314 ) = 0;
315
316 /** Implements the observation prediction \f$ h_i(x) \f$.
317 * \param 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.
318 * \param out_predictions The predicted observations.
319 */
320 virtual void OnObservationModel(
321 const mrpt::vector_size_t &idx_landmarks_to_predict,
322 vector_KFArray_OBS &out_predictions
323 ) const = 0;
324
325 /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
326 * \param 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.
327 * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
328 * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
329 */
331 const size_t &idx_landmark_to_predict,
332 KFMatrix_OxV &Hx,
333 KFMatrix_OxF &Hy
334 ) const
335 {
336 MRPT_UNUSED_PARAM(idx_landmark_to_predict); MRPT_UNUSED_PARAM(Hx); MRPT_UNUSED_PARAM(Hy);
338 }
339
340 /** 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.
341 */
343 KFArray_VEH &out_veh_increments,
344 KFArray_FEAT &out_feat_increments ) const
345 {
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;
348 }
349
350 /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
351 */
353 {
354 A -= B;
355 }
356
357
358 public:
359 /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
360 * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
361 * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
362 * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
363 * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n} \f$.
364 *
365 * - O: OBS_SIZE
366 * - V: VEH_SIZE
367 * - F: FEAT_SIZE
368 *
369 * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
370 * \deprecated This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.
371 */
373 const KFArray_OBS & in_z,
374 KFArray_FEAT & out_yn,
375 KFMatrix_FxV & out_dyn_dxv,
376 KFMatrix_FxO & out_dyn_dhn ) const
377 {
379 MRPT_UNUSED_PARAM(out_dyn_dxv); MRPT_UNUSED_PARAM(out_dyn_dhn);
381 THROW_EXCEPTION("Inverse sensor model required but not implemented in derived class.")
383 }
384
385 /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
386 * The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian),
387 * 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",
388 * the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn.
389 * Only in some problems (e.g. MonoSLAM), it'll be needed for the application to directly return the covariance matrix \a out_dyn_dhn_R_dyn_dhnT, which is the equivalent to:
390 *
391 * \f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial y_n}{\partial h_n}^\top \f$.
392 *
393 * but may be computed from additional terms, or whatever needed by the user.
394 *
395 * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
396 * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
397 * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
398 * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n} \f$.
399 * \param out_dyn_dhn_R_dyn_dhnT See the discussion above.
400 *
401 * - O: OBS_SIZE
402 * - V: VEH_SIZE
403 * - F: FEAT_SIZE
404 *
405 * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
406 */
408 const KFArray_OBS & in_z,
409 KFArray_FEAT & out_yn,
410 KFMatrix_FxV & out_dyn_dxv,
411 KFMatrix_FxO & out_dyn_dhn,
412 KFMatrix_FxF & out_dyn_dhn_R_dyn_dhnT,
413 bool & out_use_dyn_dhn_jacobian
414 ) const
415 {
416 MRPT_UNUSED_PARAM(out_dyn_dhn_R_dyn_dhnT);
418 OnInverseObservationModel(in_z,out_yn,out_dyn_dxv,out_dyn_dhn);
419 out_use_dyn_dhn_jacobian=true;
421 }
422
423 /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
424 * \param 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.
425 * \param 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.
426 * \sa OnInverseObservationModel
427 */
429 const size_t in_obsIdx,
430 const size_t in_idxNewFeat )
431 {
432 MRPT_UNUSED_PARAM(in_obsIdx); MRPT_UNUSED_PARAM(in_idxNewFeat);
433 // Do nothing in this base class.
434 }
435
436 /** 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.
437 */
439 {
440 // Do nothing in this base class.
441 }
442
443 /** This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
444 */
445 virtual void OnPostIteration()
446 {
447 // Do nothing in this base class.
448 }
449
450 /** @}
451 */
452
453 public:
454 CKalmanFilterCapable() : m_user_didnt_implement_jacobian(true) {} //!< Default constructor
455 virtual ~CKalmanFilterCapable() {} //!< Destructor
456
458
459 TKF_options KF_options; //!< Generic options for the Kalman Filter algorithm itself.
460
461
462 private:
463 // "Local" variables to runOneKalmanIteration, declared here to avoid
464 // allocating them over and over again with each call.
465 // (The variables that go into the stack remains in the function body)
468 typename mrpt::aligned_containers<KFMatrix_OxV>::vector_t Hxs; //!< The vector of all partial Jacobians dh[i]_dx for each prediction
469 typename mrpt::aligned_containers<KFMatrix_OxF>::vector_t Hys; //!< The vector of all partial Jacobians dh[i]_dy[i] for each prediction
472 vector_KFArray_OBS Z; // Each entry is one observation:
473 KFMatrix K; // Kalman gain
474 KFMatrix S_1; // Inverse of S
477
478 protected:
479
480 /** The main entry point, executes one complete step: prediction + update.
481 * It is protected since derived classes must provide a problem-specific entry point for users.
482 * The exact order in which this method calls the virtual method is explained in http://www.mrpt.org/Kalman_Filters
483 */
485
486 private:
488
489 /** Auxiliary functions for Jacobian numeric estimation */
490 static void KF_aux_estimate_trans_jacobian( const KFArray_VEH &x, const std::pair<KFCLASS*,KFArray_ACT> &dat, KFArray_VEH &out_x);
491 static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair<KFCLASS*,size_t> &dat, KFArray_OBS &out_x);
492 static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x,const std::pair<KFCLASS*,size_t> &dat,KFArray_OBS &out_x);
493
494 template <size_t VEH_SIZEb, size_t OBS_SIZEb, size_t FEAT_SIZEb, size_t ACT_SIZEb, typename KFTYPEb>
495 friend
499 const vector_int &data_association,
501 }; // end class
502
503 } // end namespace
504
505 // Specializations MUST occur at the same namespace:
506 namespace utils
507 {
508 template <>
509 struct TEnumTypeFiller<bayes::TKFMethod>
510 {
512 static void fill(bimap<enum_t,std::string> &m_map)
513 {
514 m_map.insert(bayes::kfEKFNaive, "kfEKFNaive");
515 m_map.insert(bayes::kfEKFAlaDavison, "kfEKFAlaDavison");
516 m_map.insert(bayes::kfIKFFull, "kfIKFFull");
517 m_map.insert(bayes::kfIKF, "kfIKF");
518 }
519 };
520 } // End of namespace
521} // end namespace
522
523// Template implementation:
525
526#endif
#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...
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
virtual void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const =0
Implements the transition model .
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.
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, FEAT_SIZE > KFMatrix_FxF
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 ...
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
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...
mrpt::math::CArrayNumeric< KFTYPE, OBS_SIZE > KFArray_OBS
virtual void OnTransitionNoise(KFMatrix_VxV &out_Q) const =0
Implements the transition noise covariance .
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
mrpt::math::CArrayNumeric< KFTYPE, FEAT_SIZE > KFArray_FEAT
KFTYPE kftype
The numeric type used in the Kalman Filter (default=double)
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, OBS_SIZE > KFMatrix_VxO
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...
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov) const
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
mrpt::math::CMatrixFixedNumeric< KFTYPE, ACT_SIZE, ACT_SIZE > KFMatrix_AxA
mrpt::aligned_containers< KFMatrix_OxV >::vector_t Hxs
The vector of all partial Jacobians dh[i]_dx for each prediction.
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
mrpt::math::CArrayNumeric< KFTYPE, ACT_SIZE > KFArray_ACT
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...
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 OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
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 OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
virtual void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, FEAT_SIZE > KFMatrix_VxF
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...
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
mrpt::math::CArrayNumeric< KFTYPE, VEH_SIZE > KFArray_VEH
mrpt::math::CMatrixTemplateNumeric< KFTYPE > KFMatrix
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
virtual void OnObservationModel(const mrpt::vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const =0
Implements the observation prediction .
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
Eigen::Matrix< KFTYPE, Eigen::Dynamic, 1 > KFVector
KFMatrix m_pkk
The system full covariance matrix.
mrpt::aligned_containers< KFMatrix_OxF >::vector_t Hys
The vector of all partial Jacobians dh[i]_dy[i] for each prediction.
KFVector m_xkk
The system state vector.
mrpt::utils::CTimeLogger & getProfiler()
CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > KFCLASS
My class, in a shorter name!
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
virtual void OnPostIteration()
This method is called after finishing one KF iteration and before returning from runOneKalmanIteratio...
virtual void OnGetAction(KFArray_ACT &out_u) const =0
Must return the action vector u.
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 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...
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).
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
A numeric matrix of compile-time fixed size.
This class allows loading and storing values and vectors of different types from a configuration text...
This base class provides a common printf-like method to send debug information to std::cout,...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition CStream.h:39
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
Definition CTimeLogger.h:36
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition bimap.h:29
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition bimap.h:69
TKFMethod
The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...
std::vector< int32_t > vector_int
std::vector< size_t > vector_size_t
#define MRPT_START
#define ASSERT_(f)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition mrpt_macros.h:28
#define MRPT_END
#define THROW_EXCEPTION(msg)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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)
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
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...
TKFMethod method
The method to employ (default: kfEKFNaive)
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &iniFile, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method must display clearly all the contents of the structure in textual form,...
int IKF_iterations
Number of refinement iterations, only for the IKF method.
double debug_verify_analytic_jacobians_threshold
(default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians
bool enable_profiler
If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLo...
bool verbose
If set to true timing and other information will be dumped during the execution (default=false)
bool use_analytic_transition_jacobian
(default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimate...
bool use_analytic_observation_jacobian
(default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estima...
static void fill(bimap< enum_t, std::string > &m_map)
Only specializations of this class are defined for each enum type of interest.
Definition TEnumType.h:24
A helper class that can convert an enum value into its textual representation, and viceversa.
Definition TEnumType.h:33



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 14:27:49 UTC 2023