Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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>
19 #include <mrpt/utils/CTimeLogger.h>
24 #include <mrpt/utils/stl_containers_utils.h> // find_in_vector
25 #include <mrpt/utils/CTicTac.h>
27 #include <mrpt/utils/TEnumType.h>
29 
30 
31 namespace 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 {
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  {
55  TKF_options( mrpt::utils::VerbosityLevel &verb_level_ref ) :
56  method ( kfEKFNaive),
57  verbosity_level (verb_level_ref),
58  IKF_iterations ( 5 ),
59  enable_profiler (false),
64  {
65  }
66 
68  {
69  method = iniFile.read_enum<TKFMethod>(section,"method", method );
70  verbosity_level = iniFile.read_enum<mrpt::utils::VerbosityLevel>(section,"verbosity_level", verbosity_level );
71  MRPT_LOAD_CONFIG_VAR( IKF_iterations, int , iniFile, section );
72  MRPT_LOAD_CONFIG_VAR( enable_profiler, bool , iniFile, section );
73  MRPT_LOAD_CONFIG_VAR( use_analytic_transition_jacobian, bool , iniFile, section );
74  MRPT_LOAD_CONFIG_VAR( use_analytic_observation_jacobian, 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("verbosity_level = %s\n", mrpt::utils::TEnumType<mrpt::utils::VerbosityLevel>::value2name(verbosity_level).c_str());
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  mrpt::utils::VerbosityLevel & verbosity_level;
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>
160  class CKalmanFilterCapable : public mrpt::utils::COutputLogger
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; }
167  inline size_t getNumberOfLandmarksInTheMap() const { return detail::getNumberOfLandmarksInMap(*this); }
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  */
352  virtual void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const
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  {
378  MRPT_UNUSED_PARAM(in_z); MRPT_UNUSED_PARAM(out_yn);
379  MRPT_UNUSED_PARAM(out_dyn_dxv); MRPT_UNUSED_PARAM(out_dyn_dhn);
380  MRPT_START
381  THROW_EXCEPTION("Inverse sensor model required but not implemented in derived class.")
382  MRPT_END
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);
417  MRPT_START
418  OnInverseObservationModel(in_z,out_yn,out_dyn_dxv,out_dyn_dhn);
419  out_use_dyn_dhn_jacobian=true;
420  MRPT_END
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  */
438  virtual void OnNormalizeStateVector()
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:
455  mrpt::utils::COutputLogger("CKalmanFilterCapable"),
456  KF_options(this->m_min_verbosity_level),
458  {} //!< Default constructor
459  virtual ~CKalmanFilterCapable() {} //!< Destructor
463  TKF_options KF_options; //!< Generic options for the Kalman Filter algorithm itself.
464 
465 
466  private:
467  // "Local" variables to runOneKalmanIteration, declared here to avoid
468  // allocating them over and over again with each call.
469  // (The variables that go into the stack remains in the function body)
472  typename mrpt::aligned_containers<KFMatrix_OxV>::vector_t Hxs; //!< The vector of all partial Jacobians dh[i]_dx for each prediction
473  typename mrpt::aligned_containers<KFMatrix_OxF>::vector_t Hys; //!< The vector of all partial Jacobians dh[i]_dy[i] for each prediction
476  vector_KFArray_OBS Z; // Each entry is one observation:
477  KFMatrix K; // Kalman gain
478  KFMatrix S_1; // Inverse of S
481 
482  protected:
483 
484  /** The main entry point, executes one complete step: prediction + update.
485  * It is protected since derived classes must provide a problem-specific entry point for users.
486  * The exact order in which this method calls the virtual method is explained in http://www.mrpt.org/Kalman_Filters
487  */
488  void runOneKalmanIteration();
489 
490  private:
491  mutable bool m_user_didnt_implement_jacobian;
492 
493  /** Auxiliary functions for Jacobian numeric estimation */
494  static void KF_aux_estimate_trans_jacobian( const KFArray_VEH &x, const std::pair<KFCLASS*,KFArray_ACT> &dat, KFArray_VEH &out_x);
495  static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair<KFCLASS*,size_t> &dat, KFArray_OBS &out_x);
496  static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x,const std::pair<KFCLASS*,size_t> &dat,KFArray_OBS &out_x);
497 
498  template <size_t VEH_SIZEb, size_t OBS_SIZEb, size_t FEAT_SIZEb, size_t ACT_SIZEb, typename KFTYPEb>
499  friend
503  const vector_int &data_association,
505  }; // end class
506 
507  } // end namespace
508 
509  // Specializations MUST occur at the same namespace:
510  namespace utils
511  {
512  template <>
513  struct TEnumTypeFiller<bayes::TKFMethod>
514  {
516  static void fill(bimap<enum_t,std::string> &m_map)
517  {
518  m_map.insert(bayes::kfEKFNaive, "kfEKFNaive");
519  m_map.insert(bayes::kfEKFAlaDavison, "kfEKFAlaDavison");
520  m_map.insert(bayes::kfIKFFull, "kfIKFFull");
521  m_map.insert(bayes::kfIKF, "kfIKF");
522  }
523  };
524  } // End of namespace
525 } // end namespace
526 
527 // Template implementation:
529 
530 #endif
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, FEAT_SIZE > KFMatrix_FxF
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
mrpt::math::CArrayNumeric< KFTYPE, FEAT_SIZE > KFArray_FEAT
virtual void OnInverseObservationModel(const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const
If applicable to the given problem, this method implements the inverse observation model needed to ex...
mrpt::utils::CTimeLogger & getProfiler()
bool use_analytic_observation_jacobian
(default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estima...
mrpt::math::CArrayNumeric< KFTYPE, OBS_SIZE > KFArray_OBS
virtual void 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...
static void fill(bimap< enum_t, std::string > &m_map)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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.
bool enable_profiler
If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLo...
mrpt::aligned_containers< KFMatrix_OxV >::vector_t Hxs
The vector of all partial Jacobians dh[i]_dx for each prediction.
#define THROW_EXCEPTION(msg)
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
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 runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
KFMatrix m_pkk
The system full covariance matrix.
virtual void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, mrpt::vector_size_t &out_LM_indices_to_predict) const
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
virtual void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const =0
Implements the transition model .
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
virtual void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::utils::CTimeLogger m_timLogger
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > KFCLASS
My class, in a shorter name!
A numeric matrix of compile-time fixed size.
CKalmanFilterCapable()
Default constructor.
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
#define MRPT_END
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
mrpt::math::CArrayNumeric< KFTYPE, VEH_SIZE > KFArray_VEH
A helper class that can convert an enum value into its textual representation, and viceversa...
TKF_options(mrpt::utils::VerbosityLevel &verb_level_ref)
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
mrpt::math::CArrayNumeric< KFTYPE, ACT_SIZE > KFArray_ACT
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std::...
Definition: bimap.h:28
void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov) const
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
virtual void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
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.
GLhandleARB obj
Definition: glew.h:3276
virtual ~CKalmanFilterCapable()
Destructor.
virtual void OnNewLandmarkAddedToMap(const size_t in_obsIdx, const size_t in_idxNewFeat)
If applicable to the given problem, do here any special handling of adding a new landmark to the map...
void addNewLandmarks(CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::vector_KFArray_OBS &Z, const vector_int &data_association, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO &R)
mrpt::utils::VerbosityLevel & verbosity_level
Generic options for the Kalman Filter algorithm in itself.
mrpt::aligned_containers< KFMatrix_OxF >::vector_t Hys
The vector of all partial Jacobians dh[i]_dy[i] for each prediction.
KFTYPE kftype
The numeric type used in the Kalman Filter (default=double)
virtual void OnObservationJacobiansNumericGetIncrements(KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const
Only called if using a numeric approximation of the observation Jacobians, this method must return th...
#define MRPT_START
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
virtual void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scala...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, FEAT_SIZE > KFMatrix_VxF
mrpt::math::CMatrixFixedNumeric< KFTYPE, ACT_SIZE, ACT_SIZE > KFMatrix_AxA
mrpt::math::CMatrixTemplateNumeric< KFTYPE > KFMatrix
const float R
bool debug_verify_analytic_jacobians
(default=false) If true, will compute all the Jacobians numerically and compare them to the analytica...
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
TKFMethod
The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, OBS_SIZE > KFMatrix_VxO
#define ASSERT_(f)
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
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...
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
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 OnObservationModel(const mrpt::vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const =0
Implements the observation prediction .
std::vector< int32_t > vector_int
Definition: types_simple.h:23
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.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
mrpt::math::CMatrixFixedNumeric< KFTYPE, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
void getLandmarkMean(size_t idx, KFArray_FEAT &feat) const
Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems)...
KFVector m_xkk
The system state vector.
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
bool use_analytic_transition_jacobian
(default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimate...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
int IKF_iterations
Number of refinement iterations, only for the IKF method.
virtual void OnPostIteration()
This method is called after finishing one KF iteration and before returning from runOneKalmanIteratio...
mrpt::math::CMatrixFixedNumeric< KFTYPE, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
Eigen::Matrix< KFTYPE, Eigen::Dynamic, 1 > KFVector
std::vector< int32_t > vector_int
std::vector< size_t > vector_size_t



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018