Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 
15 #include <mrpt/math/num_jacobian.h>
16 #include <mrpt/math/utils.h>
17 #include <mrpt/math/num_jacobian.h>
24 #include <mrpt/containers/stl_containers_utils.h> // find_in_vector
25 #include <mrpt/system/CTicTac.h>
29 
30 namespace mrpt
31 {
32 namespace bayes
33 {
34 /** The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable
35  * For further details on each algorithm see the tutorial:
36  * http://www.mrpt.org/Kalman_Filters
37  * \sa bayes::CKalmanFilterCapable::KF_options
38  * \ingroup mrpt_bayes_grp
39  */
41 {
46 };
47 
48 // Forward declaration:
49 template <
50  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
51  typename KFTYPE>
53 
54 /** Generic options for the Kalman Filter algorithm in itself.
55  * \ingroup mrpt_bayes_grp
56  */
58 {
60  : verbosity_level(verb_level_ref)
61  {
62  }
63 
66  const std::string& section) override
67  {
68  method = iniFile.read_enum<TKFMethod>(section, "method", method);
70  section, "verbosity_level", verbosity_level);
81  section);
82  }
83 
84  /** This method must display clearly all the contents of the structure in
85  * textual form, sending it to a CStream. */
86  void dumpToTextStream(std::ostream& out) const override
87  {
88  out << mrpt::format("\n----------- [TKF_options] ------------ \n\n");
89  out << mrpt::format(
90  "method = %s\n",
92  out << mrpt::format(
93  "verbosity_level = %s\n",
96  .c_str());
97  out << mrpt::format(
98  "IKF_iterations = %i\n", IKF_iterations);
99  out << mrpt::format(
100  "enable_profiler = %c\n",
101  enable_profiler ? 'Y' : 'N');
102  out << mrpt::format("\n");
103  }
104 
105  /** The method to employ (default: kfEKFNaive) */
108  /** Number of refinement iterations, only for the IKF method. */
110  /** If enabled (default=false), detailed timing information will be dumped
111  * to the console thru a CTimerLog at the end of the execution. */
112  bool enable_profiler{false};
113  /** (default=true) If true, OnTransitionJacobian will be called; otherwise,
114  * the Jacobian will be estimated from a numeric approximation by calling
115  * several times to OnTransitionModel. */
117  /** (default=true) If true, OnObservationJacobians will be called;
118  * otherwise, the Jacobian will be estimated from a numeric approximation by
119  * calling several times to OnObservationModel. */
121  /** (default=false) If true, will compute all the Jacobians numerically and
122  * compare them to the analytical ones, throwing an exception on mismatch.
123  */
125  /** (default-1e-2) Sets the threshold for the difference between the
126  * analytic and the numerical jacobians */
128 };
129 
130 /** Auxiliary functions, for internal usage of MRPT classes */
131 namespace detail
132 {
133 // Auxiliary functions.
134 template <
135  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
136  typename KFTYPE>
137 inline size_t getNumberOfLandmarksInMap(
139  obj);
140 // Specialization:
141 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
142 inline size_t getNumberOfLandmarksInMap(
143  const CKalmanFilterCapable<
144  VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj);
145 
146 template <
147  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
148  typename KFTYPE>
149 inline bool isMapEmpty(
151  obj);
152 // Specialization:
153 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
154 inline bool isMapEmpty(
155  const CKalmanFilterCapable<
156  VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj);
157 
158 template <
159  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
160  typename KFTYPE>
161 void addNewLandmarks(
163  const typename CKalmanFilterCapable<
164  VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
165  const std::vector<int>& data_association,
166  const typename CKalmanFilterCapable<
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>
169 void addNewLandmarks(
171  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE, KFTYPE>& obj,
172  const typename CKalmanFilterCapable<
173  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
174  KFTYPE>::vector_KFArray_OBS& Z,
175  const std::vector<int>& data_association,
176  const typename CKalmanFilterCapable<
177  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
178  KFTYPE>::KFMatrix_OxO& R);
179 } // namespace detail
180 
181 /** Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
182  * This base class stores the state vector and covariance matrix of the
183  *system. It has virtual methods that must be completed
184  * by derived classes to address a given filtering problem. The main entry
185  *point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which
186  * should be called AFTER setting the desired filter options in KF_options,
187  *as well as any options in the derived class.
188  * Note that the main entry point is protected, so derived classes must offer
189  *another method more specific to a given problem which, internally, calls
190  *runOneKalmanIteration.
191  *
192  * For further details and examples, check out the tutorial:
193  *http://www.mrpt.org/Kalman_Filters
194  *
195  * The Kalman filter algorithms are generic, but this implementation is biased
196  *to ease the implementation
197  * of SLAM-like problems. However, it can be also applied to many generic
198  *problems not related to robotics or SLAM.
199  *
200  * The meaning of the template parameters is:
201  * - VEH_SIZE: The dimension of the "vehicle state": either the full state
202  *vector or the "vehicle" part if applicable.
203  * - OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates,
204  *3 for 3D coordinates,etc).
205  * - FEAT_SIZE: The dimension of the features in the system state (the "map"),
206  *or 0 if not applicable (the default if not implemented).
207  * - ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).
208  * - KFTYPE: The numeric type of the matrices (default: double)
209  *
210  * Revisions:
211  * - 2007: Antonio J. Ortiz de Galisteo (AJOGD)
212  * - 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
213  * - 2008/MAR: Implemented IKF (JLBC).
214  * - 2009/DEC: Totally rewritten as a generic template using fixed-size
215  *matrices where possible (JLBC).
216  *
217  * \sa mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D
218  * \ingroup mrpt_bayes_grp
219  */
220 template <
221  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
222  typename KFTYPE = double>
224 {
225  public:
226  static inline size_t get_vehicle_size() { return VEH_SIZE; }
227  static inline size_t get_observation_size() { return OBS_SIZE; }
228  static inline size_t get_feature_size() { return FEAT_SIZE; }
229  static inline size_t get_action_size() { return ACT_SIZE; }
230  inline size_t getNumberOfLandmarksInTheMap() const
231  {
232  return detail::getNumberOfLandmarksInMap(*this);
233  }
234  inline bool isMapEmpty() const { return detail::isMapEmpty(*this); }
235  /** The numeric type used in the Kalman Filter (default=double) */
236  using kftype = KFTYPE;
237  /** My class, in a shorter name! */
238  using KFCLASS =
240 
241  // ---------- Many useful typedefs to short the notation a bit... --------
242  using KFVector = Eigen::Matrix<KFTYPE, Eigen::Dynamic, 1>;
244 
245  using KFMatrix_VxV =
247  using KFMatrix_OxO =
249  using KFMatrix_FxF =
251  using KFMatrix_AxA =
253 
254  using KFMatrix_VxO =
256  using KFMatrix_VxF =
258  using KFMatrix_FxV =
260  using KFMatrix_FxO =
262  using KFMatrix_OxF =
264  using KFMatrix_OxV =
266 
272 
273  inline size_t getStateVectorLength() const { return m_xkk.size(); }
274  inline KFVector& internal_getXkk() { return m_xkk; }
275  inline KFMatrix& internal_getPkk() { return m_pkk; }
276  /** Returns the mean of the estimated value of the idx'th landmark (not
277  * applicable to non-SLAM problems).
278  * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
279  */
280  inline void getLandmarkMean(size_t idx, KFArray_FEAT& feat) const
281  {
283  ::memcpy(
284  &feat[0], &m_xkk[VEH_SIZE + idx * FEAT_SIZE],
285  FEAT_SIZE * sizeof(m_xkk[0]));
286  }
287  /** Returns the covariance of the idx'th landmark (not applicable to
288  * non-SLAM problems).
289  * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
290  */
291  inline void getLandmarkCov(size_t idx, KFMatrix_FxF& feat_cov) const
292  {
293  m_pkk.extractMatrix(
294  VEH_SIZE + idx * FEAT_SIZE, VEH_SIZE + idx * FEAT_SIZE, feat_cov);
295  }
296 
297  protected:
298  /** @name Kalman filter state
299  @{ */
300 
301  /** The system state vector. */
303  /** The system full covariance matrix. */
305 
306  /** @} */
307 
309 
310  /** @name Virtual methods for Kalman Filter implementation
311  @{
312  */
313 
314  /** Must return the action vector u.
315  * \param out_u The action vector which will be passed to OnTransitionModel
316  */
317  virtual void OnGetAction(KFArray_ACT& out_u) const = 0;
318 
319  /** Implements the transition model \f$ \hat{x}_{k|k-1} = f(
320  * \hat{x}_{k-1|k-1}, u_k ) \f$
321  * \param in_u The vector returned by OnGetAction.
322  * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must
323  * have \f$ \hat{x}_{k|k-1} \f$ .
324  * \param out_skip Set this to true if for some reason you want to skip the
325  * prediction step (to do not modify either the vector or the covariance).
326  * Default:false
327  * \note Even if you return "out_skip=true", the value of "inout_x" MUST be
328  * updated as usual (this is to allow numeric approximation of Jacobians).
329  */
330  virtual void OnTransitionModel(
331  const KFArray_ACT& in_u, KFArray_VEH& inout_x,
332  bool& out_skipPrediction) const = 0;
333 
334  /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
335  * \param out_F Must return the Jacobian.
336  * The returned matrix must be \f$V \times V\f$ with V being either the
337  * size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for
338  * SLAM problems).
339  */
340  virtual void OnTransitionJacobian(KFMatrix_VxV& out_F) const
341  {
342  MRPT_UNUSED_PARAM(out_F);
344  }
345 
346  /** Only called if using a numeric approximation of the transition Jacobian,
347  * this method must return the increments in each dimension of the vehicle
348  * state vector while estimating the Jacobian.
349  */
351  KFArray_VEH& out_increments) const
352  {
353  for (size_t i = 0; i < VEH_SIZE; i++) out_increments[i] = 1e-6;
354  }
355 
356  /** Implements the transition noise covariance \f$ Q_k \f$
357  * \param out_Q Must return the covariance matrix.
358  * The returned matrix must be of the same size than the jacobian from
359  * OnTransitionJacobian
360  */
361  virtual void OnTransitionNoise(KFMatrix_VxV& out_Q) const = 0;
362 
363  /** This will be called before OnGetObservationsAndDataAssociation to allow
364  * the application to reduce the number of covariance landmark predictions
365  * to be made.
366  * For example, features which are known to be "out of sight" shouldn't be
367  * added to the output list to speed up the calculations.
368  * \param in_all_prediction_means The mean of each landmark predictions;
369  * the computation or not of the corresponding covariances is what we're
370  * trying to determined with this method.
371  * \param out_LM_indices_to_predict The list of landmark indices in the map
372  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
373  * \note This is not a pure virtual method, so it should be implemented
374  * only if desired. The default implementation returns a vector with all the
375  * landmarks in the map.
376  * \sa OnGetObservations, OnDataAssociation
377  */
379  const vector_KFArray_OBS& in_all_prediction_means,
380  std::vector<size_t>& out_LM_indices_to_predict) const
381  {
382  MRPT_UNUSED_PARAM(in_all_prediction_means);
383  // Default: all of them:
384  const size_t N = this->getNumberOfLandmarksInTheMap();
385  out_LM_indices_to_predict.resize(N);
386  for (size_t i = 0; i < N; i++) out_LM_indices_to_predict[i] = i;
387  }
388 
389  /** Return the observation NOISE covariance matrix, that is, the model of
390  * the Gaussian additive noise of the sensor.
391  * \param out_R The noise covariance matrix. It might be non diagonal, but
392  * it'll usually be.
393  * \note Upon call, it can be assumed that the previous contents of out_R
394  * are all zeros.
395  */
396  virtual void OnGetObservationNoise(KFMatrix_OxO& out_R) const = 0;
397 
398  /** This is called between the KF prediction step and the update step, and
399  * the application must return the observations and, when applicable, the
400  * data association between these observations and the current map.
401  *
402  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
403  * being the number of "observations": how many observed landmarks for a
404  * map, or just one if not applicable.
405  * \param out_data_association An empty vector or, where applicable, a
406  * vector where the i'th element corresponds to the position of the
407  * observation in the i'th row of out_z within the system state vector (in
408  * the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map
409  * element and we want to insert it at the end of this KF iteration.
410  * \param in_all_predictions A vector with the prediction of ALL the
411  * landmarks in the map. Note that, in contrast, in_S only comprises a
412  * subset of all the landmarks.
413  * \param in_S The full covariance matrix of the observation predictions
414  * (i.e. the "innovation covariance matrix"). This is a M*O x M*O matrix
415  * with M=length of "in_lm_indices_in_S".
416  * \param in_lm_indices_in_S The indices of the map landmarks (range
417  * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix
418  * in_S.
419  *
420  * This method will be called just once for each complete KF iteration.
421  * \note It is assumed that the observations are independent, i.e. there
422  * are NO cross-covariances between them.
423  */
425  vector_KFArray_OBS& out_z, std::vector<int>& out_data_association,
426  const vector_KFArray_OBS& in_all_predictions, const KFMatrix& in_S,
427  const std::vector<size_t>& in_lm_indices_in_S,
428  const KFMatrix_OxO& in_R) = 0;
429 
430  /** Implements the observation prediction \f$ h_i(x) \f$.
431  * \param idx_landmark_to_predict The indices of the landmarks in the map
432  * whose predictions are expected as output. For non SLAM-like problems,
433  * this input value is undefined and the application should just generate
434  * one observation for the given problem.
435  * \param out_predictions The predicted observations.
436  */
437  virtual void OnObservationModel(
438  const std::vector<size_t>& idx_landmarks_to_predict,
439  vector_KFArray_OBS& out_predictions) const = 0;
440 
441  /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x}
442  * \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
443  * \param idx_landmark_to_predict The index of the landmark in the map
444  * whose prediction is expected as output. For non SLAM-like problems, this
445  * will be zero and the expected output is for the whole state vector.
446  * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
447  * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i}
448  * \f$.
449  */
451  const size_t& idx_landmark_to_predict, KFMatrix_OxV& Hx,
452  KFMatrix_OxF& Hy) const
453  {
454  MRPT_UNUSED_PARAM(idx_landmark_to_predict);
455  MRPT_UNUSED_PARAM(Hx);
456  MRPT_UNUSED_PARAM(Hy);
458  }
459 
460  /** Only called if using a numeric approximation of the observation
461  * Jacobians, this method must return the increments in each dimension of
462  * the vehicle state vector while estimating the Jacobian.
463  */
465  KFArray_VEH& out_veh_increments,
466  KFArray_FEAT& out_feat_increments) const
467  {
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;
470  }
471 
472  /** Computes A=A-B, which may need to be re-implemented depending on the
473  * topology of the individual scalar components (eg, angles).
474  */
476  KFArray_OBS& A, const KFArray_OBS& B) const
477  {
478  A -= B;
479  }
480 
481  public:
482  /** If applicable to the given problem, this method implements the inverse
483  * observation model needed to extend the "map" with a new "element".
484  * \param in_z The observation vector whose inverse sensor model is to be
485  * computed. This is actually one of the vector<> returned by
486  * OnGetObservationsAndDataAssociation().
487  * \param out_yn The F-length vector with the inverse observation model \f$
488  * y_n=y(x,z_n) \f$.
489  * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor
490  * model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
491  * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor
492  * model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n}
493  * \f$.
494  *
495  * - O: OBS_SIZE
496  * - V: VEH_SIZE
497  * - F: FEAT_SIZE
498  *
499  * \note OnNewLandmarkAddedToMap will be also called after calling this
500  * method if a landmark is actually being added to the map.
501  * \deprecated This version of the method is deprecated. The alternative
502  * method is preferred to allow a greater flexibility.
503  */
505  const KFArray_OBS& in_z, KFArray_FEAT& out_yn,
506  KFMatrix_FxV& out_dyn_dxv, KFMatrix_FxO& out_dyn_dhn) const
507  {
508  MRPT_UNUSED_PARAM(in_z);
509  MRPT_UNUSED_PARAM(out_yn);
510  MRPT_UNUSED_PARAM(out_dyn_dxv);
511  MRPT_UNUSED_PARAM(out_dyn_dhn);
512  MRPT_START
514  "Inverse sensor model required but not implemented in derived "
515  "class.")
516  MRPT_END
517  }
518 
519  /** If applicable to the given problem, this method implements the inverse
520  * observation model needed to extend the "map" with a new "element".
521  * The uncertainty in the new map feature comes from two parts: one from
522  * the vehicle uncertainty (through the out_dyn_dxv Jacobian),
523  * and another from the uncertainty in the observation itself. By
524  * default, out_use_dyn_dhn_jacobian=true on call, and if it's left at
525  * "true",
526  * the base KalmanFilter class will compute the uncertainty of the
527  * landmark relative position from out_dyn_dhn.
528  * Only in some problems (e.g. MonoSLAM), it'll be needed for the
529  * application to directly return the covariance matrix \a
530  * out_dyn_dhn_R_dyn_dhnT, which is the equivalent to:
531  *
532  * \f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial
533  * y_n}{\partial h_n}^\top \f$.
534  *
535  * but may be computed from additional terms, or whatever needed by the
536  * user.
537  *
538  * \param in_z The observation vector whose inverse sensor model is to be
539  * computed. This is actually one of the vector<> returned by
540  * OnGetObservationsAndDataAssociation().
541  * \param out_yn The F-length vector with the inverse observation model \f$
542  * y_n=y(x,z_n) \f$.
543  * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor
544  * model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
545  * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor
546  * model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n}
547  * \f$.
548  * \param out_dyn_dhn_R_dyn_dhnT See the discussion above.
549  *
550  * - O: OBS_SIZE
551  * - V: VEH_SIZE
552  * - F: FEAT_SIZE
553  *
554  * \note OnNewLandmarkAddedToMap will be also called after calling this
555  * method if a landmark is actually being added to the map.
556  */
558  const KFArray_OBS& in_z, KFArray_FEAT& out_yn,
559  KFMatrix_FxV& out_dyn_dxv, KFMatrix_FxO& out_dyn_dhn,
560  KFMatrix_FxF& out_dyn_dhn_R_dyn_dhnT,
561  bool& out_use_dyn_dhn_jacobian) const
562  {
563  MRPT_UNUSED_PARAM(out_dyn_dhn_R_dyn_dhnT);
564  MRPT_START
565  OnInverseObservationModel(in_z, out_yn, out_dyn_dxv, out_dyn_dhn);
566  out_use_dyn_dhn_jacobian = true;
567  MRPT_END
568  }
569 
570  /** If applicable to the given problem, do here any special handling of
571  * adding a new landmark to the map.
572  * \param in_obsIndex The index of the observation whose inverse sensor is
573  * to be computed. It corresponds to the row in in_z where the observation
574  * can be found.
575  * \param in_idxNewFeat The index that this new feature will have in the
576  * state vector (0:just after the vehicle state, 1: after that,...). Save
577  * this number so data association can be done according to these indices.
578  * \sa OnInverseObservationModel
579  */
581  const size_t in_obsIdx, const size_t in_idxNewFeat)
582  {
583  MRPT_UNUSED_PARAM(in_obsIdx);
584  MRPT_UNUSED_PARAM(in_idxNewFeat);
585  // Do nothing in this base class.
586  }
587 
588  /** This method is called after the prediction and after the update, to give
589  * the user an opportunity to normalize the state vector (eg, keep angles
590  * within -pi,pi range) if the application requires it.
591  */
592  virtual void OnNormalizeStateVector()
593  {
594  // Do nothing in this base class.
595  }
596 
597  /** This method is called after finishing one KF iteration and before
598  * returning from runOneKalmanIteration().
599  */
600  virtual void OnPostIteration()
601  {
602  // Do nothing in this base class.
603  }
604 
605  /** @}
606  */
607 
608  public:
610  : mrpt::system::COutputLogger("CKalmanFilterCapable"),
611  KF_options(this->m_min_verbosity_level),
613  /** Default constructor */ {}
614  /** Destructor */
617  /** Generic options for the Kalman Filter algorithm itself. */
619 
620  private:
621  // "Local" variables to runOneKalmanIteration, declared here to avoid
622  // allocating them over and over again with each call.
623  // (The variables that go into the stack remains in the function body)
625  std::vector<size_t> predictLMidxs;
626  /** The vector of all partial Jacobians dh[i]_dx for each prediction */
628  /** The vector of all partial Jacobians dh[i]_dy[i] for each prediction */
632  vector_KFArray_OBS Z; // Each entry is one observation:
633  KFMatrix K; // Kalman gain
634  KFMatrix S_1; // Inverse of S
637 
638  protected:
639  /** The main entry point, executes one complete step: prediction + update.
640  * It is protected since derived classes must provide a problem-specific
641  * entry point for users.
642  * The exact order in which this method calls the virtual method is
643  * explained in http://www.mrpt.org/Kalman_Filters
644  */
645  void runOneKalmanIteration();
646 
647  private:
649 
650  /** Auxiliary functions for Jacobian numeric estimation */
651  static void KF_aux_estimate_trans_jacobian(
652  const KFArray_VEH& x, const std::pair<KFCLASS*, KFArray_ACT>& dat,
653  KFArray_VEH& out_x);
655  const KFArray_VEH& x, const std::pair<KFCLASS*, size_t>& dat,
656  KFArray_OBS& out_x);
658  const KFArray_FEAT& x, const std::pair<KFCLASS*, size_t>& dat,
659  KFArray_OBS& out_x);
660 
661  template <
662  size_t VEH_SIZEb, size_t OBS_SIZEb, size_t FEAT_SIZEb, size_t ACT_SIZEb,
663  typename KFTYPEb>
664  friend void detail::addNewLandmarks(
666  VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb>& obj,
667  const typename CKalmanFilterCapable<
668  VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb,
669  KFTYPEb>::vector_KFArray_OBS& Z,
670  const std::vector<int>& data_association,
671  const typename CKalmanFilterCapable<
672  VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb>::KFMatrix_OxO&
673  R);
674 }; // end class
675 
676 } // namespace bayes
677 } // namespace mrpt
678 
680 using namespace mrpt::bayes;
686 
687 // Template implementation:
688 #include "CKalmanFilterCapable_impl.h"
689 
690 #endif
mrpt::bayes::CKalmanFilterCapable::KF_options
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
Definition: CKalmanFilterCapable.h:618
mrpt::bayes::CKalmanFilterCapable::OnPostIteration
virtual void OnPostIteration()
This method is called after finishing one KF iteration and before returning from runOneKalmanIteratio...
Definition: CKalmanFilterCapable.h:600
mrpt::bayes::CKalmanFilterCapable::getNumberOfLandmarksInTheMap
size_t getNumberOfLandmarksInTheMap() const
Definition: CKalmanFilterCapable.h:230
mrpt::bayes::CKalmanFilterCapable::KF_aux_estimate_trans_jacobian
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.
Definition: CKalmanFilterCapable_impl.h:1204
mrpt::bayes::CKalmanFilterCapable::get_vehicle_size
static size_t get_vehicle_size()
Definition: CKalmanFilterCapable.h:226
MRPT_ENUM_TYPE_END
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:74
CMatrixFixedNumeric.h
mrpt::bayes::CKalmanFilterCapable::isMapEmpty
bool isMapEmpty() const
Definition: CKalmanFilterCapable.h:234
vector_loadsave.h
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:282
mrpt::bayes::TKF_options::method
TKFMethod method
The method to employ (default: kfEKFNaive)
Definition: CKalmanFilterCapable.h:106
mrpt::bayes::CKalmanFilterCapable::get_observation_size
static size_t get_observation_size()
Definition: CKalmanFilterCapable.h:227
mrpt::system::CTimeLogger
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
Definition: system/CTimeLogger.h:43
mrpt::bayes::CKalmanFilterCapable::get_feature_size
static size_t get_feature_size()
Definition: CKalmanFilterCapable.h:228
mrpt::bayes::TKF_options::TKF_options
TKF_options(mrpt::system::VerbosityLevel &verb_level_ref)
Definition: CKalmanFilterCapable.h:59
mrpt::bayes::CKalmanFilterCapable::m_xkk
KFVector m_xkk
The system state vector.
Definition: CKalmanFilterCapable.h:302
mrpt::bayes::CKalmanFilterCapable::Hxs
mrpt::aligned_std_vector< KFMatrix_OxV > Hxs
The vector of all partial Jacobians dh[i]_dx for each prediction.
Definition: CKalmanFilterCapable.h:627
CFileOutputStream.h
mrpt::bayes::CKalmanFilterCapable::OnTransitionJacobian
virtual void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
Definition: CKalmanFilterCapable.h:340
mrpt::bayes::CKalmanFilterCapable::OnInverseObservationModel
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...
Definition: CKalmanFilterCapable.h:504
mrpt::bayes::CKalmanFilterCapable::OnGetObservationsAndDataAssociation
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...
mrpt::bayes::TKF_options::use_analytic_transition_jacobian
bool use_analytic_transition_jacobian
(default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimate...
Definition: CKalmanFilterCapable.h:116
mrpt::bayes::CKalmanFilterCapable::S_1
KFMatrix S_1
Definition: CKalmanFilterCapable.h:634
mrpt::bayes::CKalmanFilterCapable::KFMatrix_OxO
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
Definition: CKalmanFilterCapable.h:248
mrpt::bayes::TKF_options::enable_profiler
bool enable_profiler
If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLo...
Definition: CKalmanFilterCapable.h:112
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::bayes::CKalmanFilterCapable::OnObservationJacobiansNumericGetIncrements
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...
Definition: CKalmanFilterCapable.h:464
mrpt::bayes::CKalmanFilterCapable::OnNormalizeStateVector
virtual void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
Definition: CKalmanFilterCapable.h:592
mrpt::bayes::CKalmanFilterCapable::OnPreComputingPredictions
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...
Definition: CKalmanFilterCapable.h:378
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::utils::COutputLogger
mrpt::system::COutputLogger COutputLogger
Definition: utils/COutputLogger.h:7
mrpt::bayes::CKalmanFilterCapable::Z
vector_KFArray_OBS Z
Definition: CKalmanFilterCapable.h:632
mrpt::bayes::CKalmanFilterCapable::dh_dx_full_obs
KFMatrix dh_dx_full_obs
Definition: CKalmanFilterCapable.h:635
mrpt::bayes::CKalmanFilterCapable::OnTransitionNoise
virtual void OnTransitionNoise(KFMatrix_VxV &out_Q) const =0
Implements the transition noise covariance .
R
const float R
Definition: CKinematicChain.cpp:138
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
MRPT_ENUM_TYPE_BEGIN
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:58
mrpt::bayes::kfEKFNaive
@ kfEKFNaive
Definition: CKalmanFilterCapable.h:42
mrpt::bayes::detail::isMapEmpty
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
Definition: CKalmanFilterCapable_impl.h:1406
MRPT_FILL_ENUM
MRPT_FILL_ENUM(kfEKFNaive)
mrpt::bayes::CKalmanFilterCapable::OnTransitionJacobianNumericGetIncrements
virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
Definition: CKalmanFilterCapable.h:350
mrpt::bayes::TKF_options::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &iniFile, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CKalmanFilterCapable.h:64
mrpt::math::CMatrixTemplateNumeric< double >
mrpt::bayes::CKalmanFilterCapable::Hys
mrpt::aligned_std_vector< KFMatrix_OxF > Hys
The vector of all partial Jacobians dh[i]_dy[i] for each prediction.
Definition: CKalmanFilterCapable.h:629
mrpt::aligned_std_vector
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
Definition: aligned_std_vector.h:15
mrpt::bayes::CKalmanFilterCapable::all_predictions
vector_KFArray_OBS all_predictions
Definition: CKalmanFilterCapable.h:624
mrpt::bayes::TKF_options
Generic options for the Kalman Filter algorithm in itself.
Definition: CKalmanFilterCapable.h:57
mrpt::bayes::TKF_options::debug_verify_analytic_jacobians
bool debug_verify_analytic_jacobians
(default=false) If true, will compute all the Jacobians numerically and compare them to the analytica...
Definition: CKalmanFilterCapable.h:124
CConfigFileBase.h
mrpt::bayes::CKalmanFilterCapable::m_pkk
KFMatrix m_pkk
The system full covariance matrix.
Definition: CKalmanFilterCapable.h:304
mrpt::bayes::CKalmanFilterCapable::get_action_size
static size_t get_action_size()
Definition: CKalmanFilterCapable.h:229
mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::KFVector
Eigen::Matrix< double, Eigen::Dynamic, 1 > KFVector
Definition: CKalmanFilterCapable.h:242
num_jacobian.h
mrpt::system::VerbosityLevel
VerbosityLevel
Enumeration of available verbosity levels.
Definition: system/COutputLogger.h:28
COutputLogger.h
mrpt::bayes::CKalmanFilterCapable::getStateVectorLength
size_t getStateVectorLength() const
Definition: CKalmanFilterCapable.h:273
mrpt::bayes::CKalmanFilterCapable::OnGetObservationNoise
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_START
#define MRPT_START
Definition: exceptions.h:262
CMatrixTemplateNumeric.h
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::bayes::kfEKFAlaDavison
@ kfEKFAlaDavison
Definition: CKalmanFilterCapable.h:43
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::bayes::CKalmanFilterCapable::m_user_didnt_implement_jacobian
bool m_user_didnt_implement_jacobian
Definition: CKalmanFilterCapable.h:648
mrpt::bayes::CKalmanFilterCapable::KFMatrix
mrpt::math::CMatrixTemplateNumeric< KFTYPE > KFMatrix
Definition: CKalmanFilterCapable.h:243
mrpt::bayes::TKFMethod
TKFMethod
The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...
Definition: CKalmanFilterCapable.h:40
mrpt::bayes::CKalmanFilterCapable::OnObservationJacobians
virtual void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
Definition: CKalmanFilterCapable.h:450
mrpt::bayes::CKalmanFilterCapable::OnObservationModel
virtual void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const =0
Implements the observation prediction .
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
TEnumType.h
mrpt::bayes::CKalmanFilterCapable::predictLMidxs
std::vector< size_t > predictLMidxs
Definition: CKalmanFilterCapable.h:625
mrpt::typemeta::TEnumType
A helper class that can convert an enum value into its textual representation, and viceversa.
Definition: config/CConfigFileBase.h:24
mrpt::bayes::CKalmanFilterCapable::OnNewLandmarkAddedToMap
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.
Definition: CKalmanFilterCapable.h:580
mrpt::config::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: config/CLoadableOptions.h:28
aligned_std_vector.h
mrpt::bayes::kfIKFFull
@ kfIKFFull
Definition: CKalmanFilterCapable.h:44
mrpt::bayes::CKalmanFilterCapable::KF_aux_estimate_obs_Hx_jacobian
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
Definition: CKalmanFilterCapable_impl.h:1217
mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::vector_KFArray_OBS
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
Definition: CKalmanFilterCapable.h:270
mrpt::math::CArrayNumeric
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
Definition: CArrayNumeric.h:25
mrpt::bayes::TKF_options::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method must display clearly all the contents of the structure in textual form,...
Definition: CKalmanFilterCapable.h:86
mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::kftype
double kftype
The numeric type used in the Kalman Filter (default=double)
Definition: CKalmanFilterCapable.h:236
utils.h
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:40
mrpt::bayes::CKalmanFilterCapable::internal_getXkk
KFVector & internal_getXkk()
Definition: CKalmanFilterCapable.h:274
mrpt::system::COutputLogger
Versatile class for consistent logging and management of output messages.
Definition: system/COutputLogger.h:117
mrpt::bayes::TKF_options::IKF_iterations
int IKF_iterations
Number of refinement iterations, only for the IKF method.
Definition: CKalmanFilterCapable.h:109
mrpt::bayes::TKF_options::verbosity_level
mrpt::system::VerbosityLevel & verbosity_level
Definition: CKalmanFilterCapable.h:107
mrpt::bayes::CKalmanFilterCapable::internal_getPkk
KFMatrix & internal_getPkk()
Definition: CKalmanFilterCapable.h:275
mrpt::bayes::TKF_options::debug_verify_analytic_jacobians_threshold
double debug_verify_analytic_jacobians_threshold
(default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians
Definition: CKalmanFilterCapable.h:127
mrpt::bayes::detail::getNumberOfLandmarksInMap
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
Definition: CKalmanFilterCapable_impl.h:1388
CLoadableOptions.h
mrpt::bayes::CKalmanFilterCapable::K
KFMatrix K
Definition: CKalmanFilterCapable.h:633
CTicTac.h
CTimeLogger.h
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:32
CArrayNumeric.h
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::bayes::CKalmanFilterCapable::CKalmanFilterCapable
CKalmanFilterCapable()
Default constructor.
Definition: CKalmanFilterCapable.h:609
mrpt::bayes::detail::addNewLandmarks
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)
Definition: CKalmanFilterCapable_impl.h:1256
mrpt::bayes::CKalmanFilterCapable::KF_aux_estimate_obs_Hy_jacobian
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
Definition: CKalmanFilterCapable_impl.h:1234
mrpt::bayes::CKalmanFilterCapable::~CKalmanFilterCapable
virtual ~CKalmanFilterCapable()
Destructor.
Definition: CKalmanFilterCapable.h:615
mrpt::bayes::CKalmanFilterCapable::runOneKalmanIteration
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
Definition: CKalmanFilterCapable_impl.h:27
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::bayes::CKalmanFilterCapable::OnGetAction
virtual void OnGetAction(KFArray_ACT &out_u) const =0
Must return the action vector u.
mrpt::bayes::CKalmanFilterCapable::OnTransitionModel
virtual void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const =0
Implements the transition model .
mrpt::bayes::CKalmanFilterCapable::m_timLogger
mrpt::system::CTimeLogger m_timLogger
Definition: CKalmanFilterCapable.h:308
mrpt::bayes::CKalmanFilterCapable::KFMatrix_VxV
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
Definition: CKalmanFilterCapable.h:246
mrpt::bayes::CKalmanFilterCapable::getProfiler
mrpt::system::CTimeLogger & getProfiler()
Definition: CKalmanFilterCapable.h:616
mrpt::bayes::CKalmanFilterCapable::Pkk_subset
KFMatrix Pkk_subset
Definition: CKalmanFilterCapable.h:631
mrpt::bayes::CKalmanFilterCapable::getLandmarkMean
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).
Definition: CKalmanFilterCapable.h:280
mrpt::bayes::kfIKF
@ kfIKF
Definition: CKalmanFilterCapable.h:45
mrpt::bayes::CKalmanFilterCapable::aux_K_dh_dx
KFMatrix aux_K_dh_dx
Definition: CKalmanFilterCapable.h:636
mrpt::bayes::CKalmanFilterCapable::S
KFMatrix S
Definition: CKalmanFilterCapable.h:630
mrpt::bayes::CKalmanFilterCapable::OnSubstractObservationVectors
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...
Definition: CKalmanFilterCapable.h:475
mrpt::bayes::CKalmanFilterCapable::getLandmarkCov
void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov) const
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
Definition: CKalmanFilterCapable.h:291
mrpt::bayes::CKalmanFilterCapable
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
Definition: CKalmanFilterCapable.h:52
stl_containers_utils.h
x
GLenum GLint x
Definition: glext.h:3538
mrpt::bayes::TKF_options::use_analytic_observation_jacobian
bool use_analytic_observation_jacobian
(default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estima...
Definition: CKalmanFilterCapable.h:120
mrpt::bayes::CKalmanFilterCapable::OnInverseObservationModel
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...
Definition: CKalmanFilterCapable.h:557
mrpt::system::os::memcpy
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:356



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