Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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  {
56  method ( kfEKFNaive),
57  verbose ( false ),
58  IKF_iterations ( 5 ),
59  enable_profiler (false),
64  {
65  }
66 
68  const mrpt::utils::CConfigFileBase &iniFile,
69  const std::string &section)
70  {
71  method = iniFile.read_enum<TKFMethod>(section,"method", method );
72  MRPT_LOAD_CONFIG_VAR( verbose, bool , iniFile, section );
73  MRPT_LOAD_CONFIG_VAR( IKF_iterations, int , iniFile, section );
74  MRPT_LOAD_CONFIG_VAR( enable_profiler, bool , iniFile, section );
75  MRPT_LOAD_CONFIG_VAR( use_analytic_transition_jacobian, bool , iniFile, section );
76  MRPT_LOAD_CONFIG_VAR( use_analytic_observation_jacobian, bool , iniFile, section );
77  MRPT_LOAD_CONFIG_VAR( debug_verify_analytic_jacobians, bool , iniFile, section );
79  }
80 
81  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. */
83  {
84  out.printf("\n----------- [TKF_options] ------------ \n\n");
85  out.printf("method = %s\n", mrpt::utils::TEnumType<TKFMethod>::value2name(method).c_str() );
86  out.printf("verbose = %c\n", verbose ? 'Y':'N');
87  out.printf("IKF_iterations = %i\n", IKF_iterations);
88  out.printf("enable_profiler = %c\n", enable_profiler ? 'Y':'N');
89  out.printf("\n");
90  }
91 
92 
93  TKFMethod method; //!< The method to employ (default: kfEKFNaive)
94  bool verbose; //!< If set to true timing and other information will be dumped during the execution (default=false)
95  int IKF_iterations; //!< Number of refinement iterations, only for the IKF method.
96  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.
97  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.
98  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.
99  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.
100  double debug_verify_analytic_jacobians_threshold; //!< (default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians
101  };
102 
103  /** Auxiliary functions, for internal usage of MRPT classes */
104  namespace detail
105  {
106  // Auxiliary functions.
107  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
109  // Specialization:
110  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
112 
113  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
115  // Specialization:
116  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
118 
119  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
120  void addNewLandmarks(
123  const vector_int &data_association,
125  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
126  void addNewLandmarks(
129  const vector_int &data_association,
131  }
132 
133 
134  /** Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
135  * This base class stores the state vector and covariance matrix of the system. It has virtual methods that must be completed
136  * by derived classes to address a given filtering problem. The main entry point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which
137  * should be called AFTER setting the desired filter options in KF_options, as well as any options in the derived class.
138  * 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.
139  *
140  * For further details and examples, check out the tutorial: http://www.mrpt.org/Kalman_Filters
141  *
142  * The Kalman filter algorithms are generic, but this implementation is biased to ease the implementation
143  * of SLAM-like problems. However, it can be also applied to many generic problems not related to robotics or SLAM.
144  *
145  * The meaning of the template parameters is:
146  * - VEH_SIZE: The dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
147  * - OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
148  * - FEAT_SIZE: The dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
149  * - ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).
150  * - KFTYPE: The numeric type of the matrices (default: double)
151  *
152  * Revisions:
153  * - 2007: Antonio J. Ortiz de Galisteo (AJOGD)
154  * - 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
155  * - 2008/MAR: Implemented IKF (JLBC).
156  * - 2009/DEC: Totally rewritten as a generic template using fixed-size matrices where possible (JLBC).
157  *
158  * \sa mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D
159  * \ingroup mrpt_bayes_grp
160  */
161  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
163  {
164  public:
165  static inline size_t get_vehicle_size() { return VEH_SIZE; }
166  static inline size_t get_observation_size() { return OBS_SIZE; }
167  static inline size_t get_feature_size() { return FEAT_SIZE; }
168  static inline size_t get_action_size() { return ACT_SIZE; }
169  inline size_t getNumberOfLandmarksInTheMap() const { return detail::getNumberOfLandmarksInMap(*this); }
170  inline bool isMapEmpty() const { return detail::isMapEmpty(*this); }
171 
172 
173  typedef KFTYPE kftype; //!< The numeric type used in the Kalman Filter (default=double)
175 
176  // ---------- Many useful typedefs to short the notation a bit... --------
177  typedef Eigen::Matrix<KFTYPE,Eigen::Dynamic,1> KFVector;
179 
184 
187 
190 
193 
199 
200  inline size_t getStateVectorLength() const { return m_xkk.size(); }
201 
202  inline KFVector& internal_getXkk() { return m_xkk; }
203  inline KFMatrix& internal_getPkk() { return m_pkk; }
204 
205  /** Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).
206  * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
207  */
208  inline void getLandmarkMean(size_t idx, KFArray_FEAT &feat ) const {
210  ::memcpy(&feat[0], &m_xkk[VEH_SIZE+idx*FEAT_SIZE], FEAT_SIZE*sizeof(m_xkk[0]));
211  }
212  /** Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
213  * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
214  */
215  inline void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov ) const {
216  m_pkk.extractMatrix(VEH_SIZE+idx*FEAT_SIZE,VEH_SIZE+idx*FEAT_SIZE,feat_cov);
217  }
218 
219  protected:
220  /** @name Kalman filter state
221  @{ */
222 
223  KFVector m_xkk; //!< The system state vector.
224  KFMatrix m_pkk; //!< The system full covariance matrix.
225 
226  /** @} */
227 
229 
230  /** @name Virtual methods for Kalman Filter implementation
231  @{
232  */
233 
234  /** Must return the action vector u.
235  * \param out_u The action vector which will be passed to OnTransitionModel
236  */
237  virtual void OnGetAction( KFArray_ACT &out_u ) const = 0;
238 
239  /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
240  * \param in_u The vector returned by OnGetAction.
241  * \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$ .
242  * \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
243  * \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).
244  */
245  virtual void OnTransitionModel(
246  const KFArray_ACT &in_u,
247  KFArray_VEH &inout_x,
248  bool &out_skipPrediction
249  ) const = 0;
250 
251  /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
252  * \param out_F Must return the Jacobian.
253  * 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).
254  */
255  virtual void OnTransitionJacobian( KFMatrix_VxV &out_F ) const
256  {
257  MRPT_UNUSED_PARAM(out_F);
259  }
260 
261  /** 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.
262  */
263  virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
264  {
265  for (size_t i=0;i<VEH_SIZE;i++) out_increments[i] = 1e-6;
266  }
267 
268  /** Implements the transition noise covariance \f$ Q_k \f$
269  * \param out_Q Must return the covariance matrix.
270  * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
271  */
272  virtual void OnTransitionNoise( KFMatrix_VxV &out_Q ) const = 0;
273 
274  /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
275  * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
276  * \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.
277  * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
278  * \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.
279  * \sa OnGetObservations, OnDataAssociation
280  */
282  const vector_KFArray_OBS &in_all_prediction_means,
283  mrpt::vector_size_t &out_LM_indices_to_predict ) const
284  {
285  MRPT_UNUSED_PARAM(in_all_prediction_means);
286  // Default: all of them:
287  const size_t N = this->getNumberOfLandmarksInTheMap();
288  out_LM_indices_to_predict.resize(N);
289  for (size_t i=0;i<N;i++) out_LM_indices_to_predict[i]=i;
290  }
291 
292  /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
293  * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
294  * \note Upon call, it can be assumed that the previous contents of out_R are all zeros.
295  */
296  virtual void OnGetObservationNoise(KFMatrix_OxO &out_R) const = 0;
297 
298  /** 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.
299  *
300  * \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.
301  * \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.
302  * \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.
303  * \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".
304  * \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.
305  *
306  * This method will be called just once for each complete KF iteration.
307  * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
308  */
310  vector_KFArray_OBS &out_z,
311  mrpt::vector_int &out_data_association,
312  const vector_KFArray_OBS &in_all_predictions,
313  const KFMatrix &in_S,
314  const vector_size_t &in_lm_indices_in_S,
315  const KFMatrix_OxO &in_R
316  ) = 0;
317 
318  /** Implements the observation prediction \f$ h_i(x) \f$.
319  * \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.
320  * \param out_predictions The predicted observations.
321  */
322  virtual void OnObservationModel(
323  const mrpt::vector_size_t &idx_landmarks_to_predict,
324  vector_KFArray_OBS &out_predictions
325  ) const = 0;
326 
327  /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
328  * \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.
329  * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
330  * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
331  */
333  const size_t &idx_landmark_to_predict,
334  KFMatrix_OxV &Hx,
335  KFMatrix_OxF &Hy
336  ) const
337  {
338  MRPT_UNUSED_PARAM(idx_landmark_to_predict); MRPT_UNUSED_PARAM(Hx); MRPT_UNUSED_PARAM(Hy);
340  }
341 
342  /** 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.
343  */
345  KFArray_VEH &out_veh_increments,
346  KFArray_FEAT &out_feat_increments ) const
347  {
348  for (size_t i=0;i<VEH_SIZE;i++) out_veh_increments[i] = 1e-6;
349  for (size_t i=0;i<FEAT_SIZE;i++) out_feat_increments[i] = 1e-6;
350  }
351 
352  /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
353  */
354  virtual void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const
355  {
356  A -= B;
357  }
358 
359 
360  public:
361  /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
362  * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
363  * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
364  * \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$.
365  * \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$.
366  *
367  * - O: OBS_SIZE
368  * - V: VEH_SIZE
369  * - F: FEAT_SIZE
370  *
371  * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
372  * \deprecated This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.
373  */
375  const KFArray_OBS & in_z,
376  KFArray_FEAT & out_yn,
377  KFMatrix_FxV & out_dyn_dxv,
378  KFMatrix_FxO & out_dyn_dhn ) const
379  {
380  MRPT_UNUSED_PARAM(in_z); MRPT_UNUSED_PARAM(out_yn);
381  MRPT_UNUSED_PARAM(out_dyn_dxv); MRPT_UNUSED_PARAM(out_dyn_dhn);
382  MRPT_START
383  THROW_EXCEPTION("Inverse sensor model required but not implemented in derived class.")
384  MRPT_END
385  }
386 
387  /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
388  * The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian),
389  * 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",
390  * the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn.
391  * 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:
392  *
393  * \f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial y_n}{\partial h_n}^\top \f$.
394  *
395  * but may be computed from additional terms, or whatever needed by the user.
396  *
397  * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
398  * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
399  * \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$.
400  * \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$.
401  * \param out_dyn_dhn_R_dyn_dhnT See the discussion above.
402  *
403  * - O: OBS_SIZE
404  * - V: VEH_SIZE
405  * - F: FEAT_SIZE
406  *
407  * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
408  */
410  const KFArray_OBS & in_z,
411  KFArray_FEAT & out_yn,
412  KFMatrix_FxV & out_dyn_dxv,
413  KFMatrix_FxO & out_dyn_dhn,
414  KFMatrix_FxF & out_dyn_dhn_R_dyn_dhnT,
415  bool & out_use_dyn_dhn_jacobian
416  ) const
417  {
418  MRPT_UNUSED_PARAM(out_dyn_dhn_R_dyn_dhnT);
419  MRPT_START
420  OnInverseObservationModel(in_z,out_yn,out_dyn_dxv,out_dyn_dhn);
421  out_use_dyn_dhn_jacobian=true;
422  MRPT_END
423  }
424 
425  /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
426  * \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.
427  * \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.
428  * \sa OnInverseObservationModel
429  */
431  const size_t in_obsIdx,
432  const size_t in_idxNewFeat )
433  {
434  MRPT_UNUSED_PARAM(in_obsIdx); MRPT_UNUSED_PARAM(in_idxNewFeat);
435  // Do nothing in this base class.
436  }
437 
438  /** 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.
439  */
440  virtual void OnNormalizeStateVector()
441  {
442  // Do nothing in this base class.
443  }
444 
445  /** This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
446  */
447  virtual void OnPostIteration()
448  {
449  // Do nothing in this base class.
450  }
451 
452  /** @}
453  */
454 
455  public:
456  CKalmanFilterCapable() : m_user_didnt_implement_jacobian(true) {} //!< Default constructor
457  virtual ~CKalmanFilterCapable() {} //!< Destructor
461  TKF_options KF_options; //!< Generic options for the Kalman Filter algorithm itself.
462 
463 
464  private:
465  // "Local" variables to runOneKalmanIteration, declared here to avoid
466  // allocating them over and over again with each call.
467  // (The variables that go into the stack remains in the function body)
468  vector_KFArray_OBS all_predictions;
470  typename mrpt::aligned_containers<KFMatrix_OxV>::vector_t Hxs; //!< The vector of all partial Jacobians dh[i]_dx for each prediction
471  typename mrpt::aligned_containers<KFMatrix_OxF>::vector_t Hys; //!< The vector of all partial Jacobians dh[i]_dy[i] for each prediction
472  KFMatrix S;
473  KFMatrix Pkk_subset;
474  vector_KFArray_OBS Z; // Each entry is one observation:
475  KFMatrix K; // Kalman gain
476  KFMatrix S_1; // Inverse of S
477  KFMatrix dh_dx_full_obs;
478  KFMatrix aux_K_dh_dx;
479 
480  protected:
481 
482  /** The main entry point, executes one complete step: prediction + update.
483  * It is protected since derived classes must provide a problem-specific entry point for users.
484  * The exact order in which this method calls the virtual method is explained in http://www.mrpt.org/Kalman_Filters
485  */
486  void runOneKalmanIteration();
487 
488  private:
489  mutable bool m_user_didnt_implement_jacobian;
490 
491  /** Auxiliary functions for Jacobian numeric estimation */
492  static void KF_aux_estimate_trans_jacobian( const KFArray_VEH &x, const std::pair<KFCLASS*,KFArray_ACT> &dat, KFArray_VEH &out_x);
493  static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair<KFCLASS*,size_t> &dat, KFArray_OBS &out_x);
494  static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x,const std::pair<KFCLASS*,size_t> &dat,KFArray_OBS &out_x);
495 
496  template <size_t VEH_SIZEb, size_t OBS_SIZEb, size_t FEAT_SIZEb, size_t ACT_SIZEb, typename KFTYPEb>
497  friend
501  const vector_int &data_association,
503  }; // end class
504 
505  } // end namespace
506 
507  // Specializations MUST occur at the same namespace:
508  namespace utils
509  {
510  template <>
511  struct TEnumTypeFiller<bayes::TKFMethod>
512  {
514  static void fill(bimap<enum_t,std::string> &m_map)
515  {
516  m_map.insert(bayes::kfEKFNaive, "kfEKFNaive");
517  m_map.insert(bayes::kfEKFAlaDavison, "kfEKFAlaDavison");
518  m_map.insert(bayes::kfIKFFull, "kfIKFFull");
519  m_map.insert(bayes::kfIKF, "kfIKF");
520  }
521  };
522  } // End of namespace
523 } // end namespace
524 
525 // Template implementation:
527 
528 #endif
mrpt::math::CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, FEAT_SIZE > KFMatrix_FxF
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)
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
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".
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 .
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...
bool verbose
If set to true timing and other information will be dumped during the execution (default=false) ...
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 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).
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.
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)
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.
void dumpToTextStream(mrpt::utils::CStream &out) const
This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
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
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &iniFile, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
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:35
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
This base class provides a common printf-like method to send debug information to std::cout...
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 int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
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.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Thu Dec 10 00:07:55 UTC 2015