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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 45d659fbb Tue Dec 10 18:21:14 2019 +0100 at mar dic 10 18:30:09 CET 2019