Main MRPT website > C++ reference
MRPT logo
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-2014, 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 
14 #include <mrpt/math/CArray.h>
15 #include <mrpt/math/utils.h>
16 
17 #include <mrpt/utils/CTimeLogger.h>
21 #include <mrpt/system/os.h>
22 #include <mrpt/utils/CTicTac.h>
24 #include <mrpt/utils/TEnumType.h>
25 
26 
27 namespace mrpt
28 {
29  namespace bayes
30  {
31  using namespace mrpt::utils;
32  using namespace mrpt::math;
33  using namespace mrpt;
34  using namespace std;
35 
36  /** The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable
37  * For further details on each algorithm see the tutorial: http://www.mrpt.org/Kalman_Filters
38  * \sa bayes::CKalmanFilterCapable::KF_options
39  * \ingroup mrpt_bayes_grp
40  */
41  enum TKFMethod {
46  };
47 
48  // Forward declaration:
49  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE> class CKalmanFilterCapable;
50 
51  namespace detail {
53  }
54 
55  /** Generic options for the Kalman Filter algorithm in itself.
56  * \ingroup mrpt_bayes_grp
57  */
59  {
61  method ( kfEKFNaive),
62  verbose ( false ),
63  IKF_iterations ( 5 ),
64  enable_profiler (false),
65  use_analytic_transition_jacobian (true),
66  use_analytic_observation_jacobian (true),
67  debug_verify_analytic_jacobians (false),
68  debug_verify_analytic_jacobians_threshold (1e-2)
69  {
70  }
71 
73  const mrpt::utils::CConfigFileBase &iniFile,
74  const std::string &section)
75  {
76  method = iniFile.read_enum<TKFMethod>(section,"method", method );
77  MRPT_LOAD_CONFIG_VAR( verbose, bool , iniFile, section );
78  MRPT_LOAD_CONFIG_VAR( IKF_iterations, int , iniFile, section );
79  MRPT_LOAD_CONFIG_VAR( enable_profiler, bool , iniFile, section );
80  MRPT_LOAD_CONFIG_VAR( use_analytic_transition_jacobian, bool , iniFile, section );
81  MRPT_LOAD_CONFIG_VAR( use_analytic_observation_jacobian, bool , iniFile, section );
82  MRPT_LOAD_CONFIG_VAR( debug_verify_analytic_jacobians, bool , iniFile, section );
83  MRPT_LOAD_CONFIG_VAR( debug_verify_analytic_jacobians_threshold, double, iniFile, section );
84  }
85 
86  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. */
87  void dumpToTextStream(CStream &out) const
88  {
89  out.printf("\n----------- [TKF_options] ------------ \n\n");
90  out.printf("method = %s\n", mrpt::utils::TEnumType<TKFMethod>::value2name(method).c_str() );
91  out.printf("verbose = %c\n", verbose ? 'Y':'N');
92  out.printf("IKF_iterations = %i\n", IKF_iterations);
93  out.printf("enable_profiler = %c\n", enable_profiler ? 'Y':'N');
94  out.printf("\n");
95  }
96 
97 
98  TKFMethod method; //!< The method to employ (default: kfEKFNaive)
99  bool verbose; //!< If set to true timing and other information will be dumped during the execution (default=false)
100  int IKF_iterations; //!< Number of refinement iterations, only for the IKF method.
101  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.
102  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.
103  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.
104  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.
105  double debug_verify_analytic_jacobians_threshold; //!< (default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians
106  };
107 
108  /** Auxiliary functions, for internal usage of MRPT classes */
109  namespace detail
110  {
111  // Auxiliary functions.
112  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
114  // Specialization:
115  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
117 
118  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
120  // Specialization:
121  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
123  }
124 
125 
126  /** Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
127  * This base class stores the state vector and covariance matrix of the system. It has virtual methods that must be completed
128  * by derived classes to address a given filtering problem. The main entry point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which
129  * should be called AFTER setting the desired filter options in KF_options, as well as any options in the derived class.
130  * 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.
131  *
132  * For further details and examples, check out the tutorial: http://www.mrpt.org/Kalman_Filters
133  *
134  * The Kalman filter algorithms are generic, but this implementation is biased to ease the implementation
135  * of SLAM-like problems. However, it can be also applied to many generic problems not related to robotics or SLAM.
136  *
137  * The meaning of the template parameters is:
138  * - VEH_SIZE: The dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
139  * - OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
140  * - FEAT_SIZE: The dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
141  * - ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).
142  * - KFTYPE: The numeric type of the matrices (default: double)
143  *
144  * Revisions:
145  * - 2007: Antonio J. Ortiz de Galisteo (AJOGD)
146  * - 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
147  * - 2008/MAR: Implemented IKF (JLBC).
148  * - 2009/DEC: Totally rewritten as a generic template using fixed-size matrices where possible (JLBC).
149  *
150  * \sa mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D
151  * \ingroup mrpt_bayes_grp
152  */
153  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
154  class CKalmanFilterCapable : public mrpt::utils::CDebugOutputCapable
155  {
156  public:
157  static inline size_t get_vehicle_size() { return VEH_SIZE; }
158  static inline size_t get_observation_size() { return OBS_SIZE; }
159  static inline size_t get_feature_size() { return FEAT_SIZE; }
160  static inline size_t get_action_size() { return ACT_SIZE; }
161  inline size_t getNumberOfLandmarksInTheMap() const { return detail::getNumberOfLandmarksInMap(*this); }
162  inline bool isMapEmpty() const { return detail::isMapEmpty(*this); }
163 
164 
165  typedef KFTYPE kftype; //!< The numeric type used in the Kalman Filter (default=double)
167 
168  // ---------- Many useful typedefs to short the notation a bit... --------
171 
176 
179 
182 
185 
191 
192  inline size_t getStateVectorLength() const { return m_xkk.size(); }
193 
194  /** Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).
195  * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
196  */
197  inline void getLandmarkMean(size_t idx, KFArray_FEAT &feat ) const {
198  ASSERT_(idx<getNumberOfLandmarksInTheMap())
199  ::memcpy(&feat[0], &m_xkk[VEH_SIZE+idx*FEAT_SIZE], FEAT_SIZE*sizeof(m_xkk[0]));
200  }
201  /** Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
202  * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
203  */
204  inline void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov ) const {
205  m_pkk.extractMatrix(VEH_SIZE+idx*FEAT_SIZE,VEH_SIZE+idx*FEAT_SIZE,feat_cov);
206  }
207 
208  protected:
209  /** @name Kalman filter state
210  @{ */
211 
212  KFVector m_xkk; //!< The system state vector.
213  KFMatrix m_pkk; //!< The system full covariance matrix.
214 
215  /** @} */
216 
218 
219  /** @name Virtual methods for Kalman Filter implementation
220  @{
221  */
222 
223  /** Must return the action vector u.
224  * \param out_u The action vector which will be passed to OnTransitionModel
225  */
226  virtual void OnGetAction( KFArray_ACT &out_u ) const = 0;
227 
228  /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
229  * \param in_u The vector returned by OnGetAction.
230  * \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$ .
231  * \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
232  * \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).
233  */
234  virtual void OnTransitionModel(
235  const KFArray_ACT &in_u,
236  KFArray_VEH &inout_x,
237  bool &out_skipPrediction
238  ) const = 0;
239 
240  /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
241  * \param out_F Must return the Jacobian.
242  * 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).
243  */
244  virtual void OnTransitionJacobian( KFMatrix_VxV &out_F ) const
245  {
246  m_user_didnt_implement_jacobian=true;
247  }
248 
249  /** 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.
250  */
251  virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
252  {
253  for (size_t i=0;i<VEH_SIZE;i++) out_increments[i] = 1e-6;
254  }
255 
256  /** Implements the transition noise covariance \f$ Q_k \f$
257  * \param out_Q Must return the covariance matrix.
258  * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
259  */
260  virtual void OnTransitionNoise( KFMatrix_VxV &out_Q ) const = 0;
261 
262  /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
263  * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
264  * \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.
265  * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
266  * \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.
267  * \sa OnGetObservations, OnDataAssociation
268  */
270  const vector_KFArray_OBS &in_all_prediction_means,
271  mrpt::vector_size_t &out_LM_indices_to_predict ) const
272  {
273  // Default: all of them:
274  const size_t N = this->getNumberOfLandmarksInTheMap();
275  out_LM_indices_to_predict.resize(N);
276  for (size_t i=0;i<N;i++) out_LM_indices_to_predict[i]=i;
277  }
278 
279  /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
280  * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
281  * \note Upon call, it can be assumed that the previous contents of out_R are all zeros.
282  */
283  virtual void OnGetObservationNoise(KFMatrix_OxO &out_R) const = 0;
284 
285  /** 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.
286  *
287  * \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.
288  * \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.
289  * \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.
290  * \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".
291  * \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.
292  *
293  * This method will be called just once for each complete KF iteration.
294  * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
295  */
296  virtual void OnGetObservationsAndDataAssociation(
297  vector_KFArray_OBS &out_z,
298  mrpt::vector_int &out_data_association,
299  const vector_KFArray_OBS &in_all_predictions,
300  const KFMatrix &in_S,
301  const vector_size_t &in_lm_indices_in_S,
302  const KFMatrix_OxO &in_R
303  ) = 0;
304 
305  /** Implements the observation prediction \f$ h_i(x) \f$.
306  * \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.
307  * \param out_predictions The predicted observations.
308  */
309  virtual void OnObservationModel(
310  const mrpt::vector_size_t &idx_landmarks_to_predict,
311  vector_KFArray_OBS &out_predictions
312  ) const = 0;
313 
314  /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
315  * \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.
316  * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
317  * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
318  */
320  const size_t &idx_landmark_to_predict,
321  KFMatrix_OxV &Hx,
322  KFMatrix_OxF &Hy
323  ) const
324  {
325  m_user_didnt_implement_jacobian=true;
326  }
327 
328  /** 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.
329  */
331  KFArray_VEH &out_veh_increments,
332  KFArray_FEAT &out_feat_increments ) const
333  {
334  for (size_t i=0;i<VEH_SIZE;i++) out_veh_increments[i] = 1e-6;
335  for (size_t i=0;i<FEAT_SIZE;i++) out_feat_increments[i] = 1e-6;
336  }
337 
338  /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
339  */
340  virtual void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const
341  {
342  A -= B;
343  }
344 
345  /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
346  * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
347  * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
348  * \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$.
349  * \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$.
350  *
351  * - O: OBS_SIZE
352  * - V: VEH_SIZE
353  * - F: FEAT_SIZE
354  *
355  * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
356  * \deprecated This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.
357  */
359  const KFArray_OBS & in_z,
360  KFArray_FEAT & out_yn,
361  KFMatrix_FxV & out_dyn_dxv,
362  KFMatrix_FxO & out_dyn_dhn ) const
363  {
364  MRPT_START
365  THROW_EXCEPTION("Inverse sensor model required but not implemented in derived class.")
366  MRPT_END
367  }
368 
369  /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
370  * The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian),
371  * 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",
372  * the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn.
373  * 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:
374  *
375  * \f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial y_n}{\partial h_n}^\top \f$.
376  *
377  * but may be computed from additional terms, or whatever needed by the user.
378  *
379  * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
380  * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
381  * \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$.
382  * \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$.
383  * \param out_dyn_dhn_R_dyn_dhnT See the discussion above.
384  *
385  * - O: OBS_SIZE
386  * - V: VEH_SIZE
387  * - F: FEAT_SIZE
388  *
389  * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
390  */
392  const KFArray_OBS & in_z,
393  KFArray_FEAT & out_yn,
394  KFMatrix_FxV & out_dyn_dxv,
395  KFMatrix_FxO & out_dyn_dhn,
396  KFMatrix_FxF & out_dyn_dhn_R_dyn_dhnT,
397  bool & out_use_dyn_dhn_jacobian
398  ) const
399  {
400  MRPT_START
401  OnInverseObservationModel(in_z,out_yn,out_dyn_dxv,out_dyn_dhn);
402  out_use_dyn_dhn_jacobian=true;
403  MRPT_END
404  }
405 
406  /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
407  * \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.
408  * \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.
409  * \sa OnInverseObservationModel
410  */
412  const size_t in_obsIdx,
413  const size_t in_idxNewFeat )
414  {
415  // Do nothing in this base class.
416  }
417 
418  /** 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.
419  */
420  virtual void OnNormalizeStateVector()
421  {
422  // Do nothing in this base class.
423  }
424 
425  /** This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
426  */
427  virtual void OnPostIteration()
428  {
429  // Do nothing in this base class.
430  }
431 
432  /** @}
433  */
434 
435  public:
436  CKalmanFilterCapable() {} //!< Default constructor
437  virtual ~CKalmanFilterCapable() {} //!< Destructor
439  mrpt::utils::CTimeLogger &getProfiler() { return m_timLogger; }
441  TKF_options KF_options; //!< Generic options for the Kalman Filter algorithm itself.
442 
443 
444  private:
445  // "Local" variables to runOneKalmanIteration, declared here to avoid
446  // allocating them over and over again with each call.
447  // (The variables that go into the stack remains in the function body)
448  vector_KFArray_OBS all_predictions;
449  vector_size_t predictLMidxs;
450  typename mrpt::aligned_containers<KFMatrix_OxV>::vector_t Hxs; //!< The vector of all partial Jacobians dh[i]_dx for each prediction
451  typename mrpt::aligned_containers<KFMatrix_OxF>::vector_t Hys; //!< The vector of all partial Jacobians dh[i]_dy[i] for each prediction
453  KFMatrix Pkk_subset;
454  vector_KFArray_OBS Z; // Each entry is one observation:
455  KFMatrix K; // Kalman gain
456  KFMatrix S_1; // Inverse of S
457  KFMatrix dh_dx_full_obs;
458  KFMatrix aux_K_dh_dx;
459 
460  protected:
461 
462  /** The main entry point, executes one complete step: prediction + update.
463  * It is protected since derived classes must provide a problem-specific entry point for users.
464  * The exact order in which this method calls the virtual method is explained in http://www.mrpt.org/Kalman_Filters
465  */
466  void runOneKalmanIteration()
467  {
468  MRPT_START
469 
470  m_timLogger.enable(KF_options.enable_profiler || KF_options.verbose);
471  m_timLogger.enter("KF:complete_step");
472 
473  ASSERT_(size_t(m_xkk.size())==m_pkk.getColCount())
474  ASSERT_(size_t(m_xkk.size())>=VEH_SIZE)
475 
476  // =============================================================
477  // 1. CREATE ACTION MATRIX u FROM ODOMETRY
478  // =============================================================
479  KFArray_ACT u;
480 
481  m_timLogger.enter("KF:1.OnGetAction");
482  OnGetAction(u);
483  m_timLogger.leave("KF:1.OnGetAction");
484 
485  // Sanity check:
486  if (FEAT_SIZE) { ASSERTDEB_( (((m_xkk.size()-VEH_SIZE)/FEAT_SIZE)*FEAT_SIZE)== (m_xkk.size()-VEH_SIZE) ) }
487 
488  // =============================================================
489  // 2. PREDICTION OF NEW POSE xv_{k+1|k}
490  // =============================================================
491  m_timLogger.enter("KF:2.prediction stage");
492 
493  const size_t N_map = getNumberOfLandmarksInTheMap();
494 
495  KFArray_VEH xv( &m_xkk[0] ); // Vehicle pose
496 
497  bool skipPrediction=false; // Wether to skip the prediction step (in SLAM this is desired for the first iteration...)
498 
499  // Update mean: xv will have the updated pose until we update it in the filterState later.
500  // This is to maintain a copy of the last robot pose in the state vector, required for the Jacobian computation.
501  OnTransitionModel(u, xv, skipPrediction);
502 
503  if ( !skipPrediction )
504  {
505  // =============================================================
506  // 3. PREDICTION OF COVARIANCE P_{k+1|k}
507  // =============================================================
508  // First, we compute de Jacobian fv_by_xv (derivative of f_vehicle wrt x_vehicle):
509  KFMatrix_VxV dfv_dxv;
510 
511  // Try closed-form Jacobian first:
512  m_user_didnt_implement_jacobian=false; // Set to true by the default method if not reimplemented in base class.
513  if (KF_options.use_analytic_transition_jacobian)
514  OnTransitionJacobian(dfv_dxv);
515 
516  if (m_user_didnt_implement_jacobian || !KF_options.use_analytic_transition_jacobian || KF_options.debug_verify_analytic_jacobians)
517  { // Numeric approximation:
518  KFArray_VEH xkk_vehicle( &m_xkk[0] ); // A copy of the vehicle part of the state vector.
519  KFArray_VEH xkk_veh_increments;
520  OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
521 
523  xkk_vehicle,
524  &KF_aux_estimate_trans_jacobian, //(const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3 &out),
525  xkk_veh_increments,
526  std::pair<KFCLASS*,KFArray_ACT>(this,u),
527  dfv_dxv);
528 
529  if (KF_options.debug_verify_analytic_jacobians)
530  {
532  OnTransitionJacobian(dfv_dxv_gt);
533  if ((dfv_dxv-dfv_dxv_gt).Abs().sumAll()>KF_options.debug_verify_analytic_jacobians_threshold)
534  {
535  std::cerr << "[KalmanFilter] ERROR: User analytical transition Jacobians are wrong: \n"
536  << " Real dfv_dxv: \n" << dfv_dxv << "\n Analytical dfv_dxv:\n" << dfv_dxv_gt << "Diff:\n" << (dfv_dxv-dfv_dxv_gt) << "\n";
537  THROW_EXCEPTION("ERROR: User analytical transition Jacobians are wrong (More details dumped to cerr)")
538  }
539  }
540 
541  }
542 
543  // Q is the process noise covariance matrix, is associated to the robot movement and is necesary to calculate the prediction P(k+1|k)
544  KFMatrix_VxV Q;
545  OnTransitionNoise(Q);
546 
547  // ====================================
548  // 3.1: Pxx submatrix
549  // ====================================
550  // Replace old covariance:
551  Eigen::Block<typename KFMatrix::Base,VEH_SIZE,VEH_SIZE>(m_pkk,0,0) =
552  Q +
553  dfv_dxv * Eigen::Block<typename KFMatrix::Base,VEH_SIZE,VEH_SIZE>(m_pkk,0,0) * dfv_dxv.transpose();
554 
555  // ====================================
556  // 3.2: All Pxy_i
557  // ====================================
558  // Now, update the cov. of landmarks, if any:
559  KFMatrix_VxF aux;
560  for (size_t i=0 ; i<N_map ; i++)
561  {
562  aux = dfv_dxv * Eigen::Block<typename KFMatrix::Base,VEH_SIZE,FEAT_SIZE>(m_pkk,0,VEH_SIZE+i*FEAT_SIZE);
563 
564  Eigen::Block<typename KFMatrix::Base,VEH_SIZE,FEAT_SIZE>(m_pkk, 0 , VEH_SIZE+i*FEAT_SIZE) = aux;
565  Eigen::Block<typename KFMatrix::Base,FEAT_SIZE,VEH_SIZE>(m_pkk, VEH_SIZE+i*FEAT_SIZE , 0 ) = aux.transpose();
566  }
567 
568  // =============================================================
569  // 4. NOW WE CAN OVERWRITE THE NEW STATE VECTOR
570  // =============================================================
571  for (size_t i=0;i<VEH_SIZE;i++)
572  m_xkk[i]=xv[i];
573 
574  // Normalize, if neccesary.
575  OnNormalizeStateVector();
576 
577  } // end if (!skipPrediction)
578 
579  const double tim_pred = m_timLogger.leave("KF:2.prediction stage");
580 
581  // =============================================================
582  // 5. PREDICTION OF OBSERVATIONS AND COMPUTE JACOBIANS
583  // =============================================================
584  m_timLogger.enter("KF:3.predict all obs");
585 
586  KFMatrix_OxO R; // Sensor uncertainty (covariance matrix): R
587  OnGetObservationNoise(R);
588 
589  // Predict the observations for all the map LMs, so the user
590  // can decide if their covariances (more costly) must be computed as well:
591  all_predictions.resize(N_map);
592  OnObservationModel(
593  mrpt::math::sequenceStdVec<size_t,1>(0,N_map),
594  all_predictions);
595 
596  const double tim_pred_obs = m_timLogger.leave("KF:3.predict all obs");
597 
598  m_timLogger.enter("KF:4.decide pred obs");
599 
600  // Decide if some of the covariances shouldn't be predicted.
601  predictLMidxs.clear();
602  if (FEAT_SIZE==0)
603  // In non-SLAM problems, just do one prediction, for the entire system state:
604  predictLMidxs.assign(1,0);
605  else
606  // On normal SLAM problems:
607  OnPreComputingPredictions(all_predictions, predictLMidxs);
608 
609 
610  m_timLogger.leave("KF:4.decide pred obs");
611 
612  // =============================================================
613  // 6. COMPUTE INNOVATION MATRIX "S"
614  // =============================================================
615  // Do the prediction of the observation covariances:
616  // Compute S: S = H P ~H + R
617  //
618  // Build a big dh_dx Jacobian composed of the small block Jacobians.
619  // , but: it's actually a subset of the full Jacobian, since the non-predicted
620  // features do not appear.
621  //
622  // dh_dx: O·M x V+M·F
623  // S: O·M x O·M
624  // M = |predictLMidxs|
625  // O=size of each observation.
626  // F=size of features in the map
627  //
628  // Updated: Now we only keep the non-zero blocks of that Jacobian,
629  // in the vectors Hxs[] and Hys[].
630  //
631 
632  m_timLogger.enter("KF:5.build Jacobians");
633 
634  size_t N_pred = FEAT_SIZE==0 ?
635  1 /* In non-SLAM problems, there'll be only 1 fixed observation */ :
636  predictLMidxs.size();
637 
638  vector_int data_association; // -1: New map feature.>=0: Indexes in the state vector
639 
640  // The next loop will only do more than one iteration if the heuristic in OnPreComputingPredictions() fails,
641  // which will be detected by the addition of extra landmarks to predict into "missing_predictions_to_add"
642  std::vector<size_t> missing_predictions_to_add;
643 
644  Hxs.resize(N_pred); // Lists of partial Jacobians
645  Hys.resize(N_pred);
646 
647  size_t first_new_pred = 0; // This will be >0 only if we perform multiple loops due to failures in the prediction heuristic.
648 
649  do
650  {
651  if (!missing_predictions_to_add.empty())
652  {
653  const size_t nNew = missing_predictions_to_add.size();
654  printf_debug("[KF] *Performance Warning*: %u LMs were not correctly predicted by OnPreComputingPredictions()\n",static_cast<unsigned int>(nNew));
655 
656  ASSERTDEB_(FEAT_SIZE!=0)
657  for (size_t j=0;j<nNew;j++)
658  predictLMidxs.push_back( missing_predictions_to_add[j] );
659 
660  N_pred = predictLMidxs.size();
661  missing_predictions_to_add.clear();
662  }
663 
664  Hxs.resize(N_pred); // Append new entries, if needed.
665  Hys.resize(N_pred);
666 
667  for (size_t i=first_new_pred;i<N_pred;++i)
668  {
669  const size_t lm_idx = FEAT_SIZE==0 ? 0 : predictLMidxs[i];
670  KFMatrix_OxV &Hx = Hxs[i];
671  KFMatrix_OxF &Hy = Hys[i];
672 
673  // Try the analitic Jacobian first:
674  m_user_didnt_implement_jacobian=false; // Set to true by the default method if not reimplemented in base class.
675  if (KF_options.use_analytic_observation_jacobian)
676  OnObservationJacobians(lm_idx,Hx,Hy);
677 
678  if (m_user_didnt_implement_jacobian || !KF_options.use_analytic_observation_jacobian || KF_options.debug_verify_analytic_jacobians)
679  { // Numeric approximation:
680  const size_t lm_idx_in_statevector = VEH_SIZE+lm_idx*FEAT_SIZE;
681 
682  const KFArray_VEH x_vehicle( &m_xkk[0] );
683  const KFArray_FEAT x_feat( &m_xkk[lm_idx_in_statevector] );
684 
685  KFArray_VEH xkk_veh_increments;
686  KFArray_FEAT feat_increments;
687  OnObservationJacobiansNumericGetIncrements(xkk_veh_increments, feat_increments);
688 
690  x_vehicle,
691  &KF_aux_estimate_obs_Hx_jacobian,
692  xkk_veh_increments,
693  std::pair<KFCLASS*,size_t>(this,lm_idx),
694  Hx);
695  // The state vector was temporarily modified by KF_aux_estimate_*, restore it:
696  ::memcpy(&m_xkk[0],&x_vehicle[0],sizeof(m_xkk[0])*VEH_SIZE);
697 
699  x_feat,
700  &KF_aux_estimate_obs_Hy_jacobian,
701  feat_increments,
702  std::pair<KFCLASS*,size_t>(this,lm_idx),
703  Hy);
704  // The state vector was temporarily modified by KF_aux_estimate_*, restore it:
705  ::memcpy(&m_xkk[lm_idx_in_statevector],&x_feat[0],sizeof(m_xkk[0])*FEAT_SIZE);
706 
707  if (KF_options.debug_verify_analytic_jacobians)
708  {
711  OnObservationJacobians(lm_idx,Hx_gt,Hy_gt);
712  if ((Hx-Hx_gt).Abs().sumAll()>KF_options.debug_verify_analytic_jacobians_threshold) {
713  std::cerr << "[KalmanFilter] ERROR: User analytical observation Hx Jacobians are wrong: \n"
714  << " Real Hx: \n" << Hx << "\n Analytical Hx:\n" << Hx_gt << "Diff:\n" << Hx-Hx_gt << "\n";
715  THROW_EXCEPTION("ERROR: User analytical observation Hx Jacobians are wrong (More details dumped to cerr)")
716  }
717  if ((Hy-Hy_gt).Abs().sumAll()>KF_options.debug_verify_analytic_jacobians_threshold) {
718  std::cerr << "[KalmanFilter] ERROR: User analytical observation Hy Jacobians are wrong: \n"
719  << " Real Hy: \n" << Hy << "\n Analytical Hx:\n" << Hy_gt << "Diff:\n" << Hy-Hy_gt << "\n";
720  THROW_EXCEPTION("ERROR: User analytical observation Hy Jacobians are wrong (More details dumped to cerr)")
721  }
722  }
723  }
724  }
725  m_timLogger.leave("KF:5.build Jacobians");
726 
727  m_timLogger.enter("KF:6.build S");
728 
729  // Compute S: S = H P ~H + R (R will be added below)
730  // exploiting the sparsity of H:
731  // Each block in S is:
732  // Sij =
733  // ------------------------------------------
734  S.setSize(N_pred*OBS_SIZE,N_pred*OBS_SIZE);
735 
736  if ( FEAT_SIZE>0 )
737  { // SLAM-like problem:
738  const Eigen::Block<const typename KFMatrix::Base,VEH_SIZE,VEH_SIZE> Px(m_pkk,0,0); // Covariance of the vehicle pose
739 
740  for (size_t i=0;i<N_pred;++i)
741  {
742  const size_t lm_idx_i = predictLMidxs[i];
743  const Eigen::Block<const typename KFMatrix::Base,FEAT_SIZE,VEH_SIZE> Pxyi_t(m_pkk,VEH_SIZE+lm_idx_i*FEAT_SIZE,0); // Pxyi^t
744 
745  // Only do j>=i (upper triangle), since S is symmetric:
746  for (size_t j=i;j<N_pred;++j)
747  {
748  const size_t lm_idx_j = predictLMidxs[j];
749  // Sij block:
750  Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE> Sij(S,OBS_SIZE*i,OBS_SIZE*j);
751 
752  const Eigen::Block<const typename KFMatrix::Base,VEH_SIZE,FEAT_SIZE> Pxyj(m_pkk,0, VEH_SIZE+lm_idx_j*FEAT_SIZE);
753  const Eigen::Block<const typename KFMatrix::Base,FEAT_SIZE,FEAT_SIZE> Pyiyj(m_pkk,VEH_SIZE+lm_idx_i*FEAT_SIZE,VEH_SIZE+lm_idx_j*FEAT_SIZE);
754 
755  Sij = Hxs[i] * Px * Hxs[j].transpose()
756  + Hys[i] * Pxyi_t * Hxs[j].transpose()
757  + Hxs[i] * Pxyj * Hys[j].transpose()
758  + Hys[i] * Pyiyj * Hys[j].transpose();
759 
760  // Copy transposed to the symmetric lower-triangular part:
761  if (i!=j)
762  Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(S,OBS_SIZE*j,OBS_SIZE*i) = Sij.transpose();
763  }
764 
765  // Sum the "R" term to the diagonal blocks:
766  const size_t obs_idx_off = i*OBS_SIZE;
767  Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(S,obs_idx_off,obs_idx_off) += R;
768  }
769  }
770  else
771  { // Not SLAM-like problem: simply S=H*Pkk*H^t + R
772  ASSERTDEB_(N_pred==1)
773  ASSERTDEB_(S.getColCount() == OBS_SIZE )
774 
775  S = Hxs[0] * m_pkk *Hxs[0].transpose() + R;
776  }
777 
778  m_timLogger.leave("KF:6.build S");
779 
780  Z.clear(); // Each entry is one observation:
781 
782  m_timLogger.enter("KF:7.get obs & DA");
783 
784  // Get observations and do data-association:
785  OnGetObservationsAndDataAssociation(
786  Z, data_association, // Out
787  all_predictions, S, predictLMidxs, R // In
788  );
789  ASSERTDEB_(data_association.size()==Z.size() || (data_association.empty() && FEAT_SIZE==0));
790 
791  // Check if an observation hasn't been predicted in OnPreComputingPredictions() but has been actually
792  // observed. This may imply an error in the heuristic of OnPreComputingPredictions(), and forces us
793  // to rebuild the matrices
794  missing_predictions_to_add.clear();
795  if (FEAT_SIZE!=0)
796  {
797  for (size_t i=0;i<data_association.size();++i)
798  {
799  if (data_association[i]<0) continue;
800  const size_t assoc_idx_in_map = static_cast<size_t>(data_association[i]);
801  const size_t assoc_idx_in_pred = mrpt::utils::find_in_vector(assoc_idx_in_map, predictLMidxs);
802  if (assoc_idx_in_pred==std::string::npos)
803  missing_predictions_to_add.push_back(assoc_idx_in_map);
804  }
805  }
806 
807  first_new_pred = N_pred; // If we do another loop, start at the begin of new predictions
808 
809  } while (!missing_predictions_to_add.empty());
810 
811 
812  const double tim_obs_DA = m_timLogger.leave("KF:7.get obs & DA");
813 
814  // =============================================================
815  // 7. UPDATE USING THE KALMAN GAIN
816  // =============================================================
817  // Update, only if there are observations!
818  if ( !Z.empty() )
819  {
820  m_timLogger.enter("KF:8.update stage");
821 
822  switch (KF_options.method)
823  {
824  // -----------------------
825  // FULL KF- METHOD
826  // -----------------------
827  case kfEKFNaive:
828  case kfIKFFull:
829  {
830  // Build the whole Jacobian dh_dx matrix
831  // ---------------------------------------------
832  // Keep only those whose DA is not -1
833  vector_int mapIndicesForKFUpdate(data_association.size());
834  mapIndicesForKFUpdate.resize( std::distance(mapIndicesForKFUpdate.begin(),
835  std::remove_copy_if(
836  data_association.begin(),
837  data_association.end(),
838  mapIndicesForKFUpdate.begin(),
839  binder1st<equal_to<int> >(equal_to<int>(),-1) ) ) );
840 
841  const size_t N_upd = (FEAT_SIZE==0) ?
842  1 : // Non-SLAM problems: Just one observation for the entire system.
843  mapIndicesForKFUpdate.size(); // SLAM: # of observed known landmarks
844 
845  // Just one, or several update iterations??
846  const size_t nKF_iterations = (KF_options.method==kfEKFNaive) ? 1 : KF_options.IKF_iterations;
847 
848  const KFVector xkk_0 = m_xkk;
849 
850  // For each IKF iteration (or 1 for EKF)
851  if (N_upd>0) // Do not update if we have no observations!
852  {
853  for (size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
854  {
855  // Compute ytilde = OBS - PREDICTION
856  KFVector ytilde(OBS_SIZE*N_upd);
857  size_t ytilde_idx = 0;
858 
859  // TODO: Use a Matrix view of "dh_dx_full" instead of creating a copy into "dh_dx_full_obs"
860  dh_dx_full_obs.zeros(N_upd*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_map ); // Init to zeros.
861  KFMatrix S_observed; // The KF "S" matrix: A re-ordered, subset, version of the prediction S:
862 
863  if (FEAT_SIZE!=0)
864  { // SLAM problems:
865  vector_size_t S_idxs;
866  S_idxs.reserve(OBS_SIZE*N_upd);
867 
868  //const size_t row_len = VEH_SIZE + FEAT_SIZE * N_map;
869 
870  for (size_t i=0;i<data_association.size();++i)
871  {
872  if (data_association[i]<0) continue;
873 
874  const size_t assoc_idx_in_map = static_cast<size_t>(data_association[i]);
875  const size_t assoc_idx_in_pred = mrpt::utils::find_in_vector(assoc_idx_in_map, predictLMidxs);
876  ASSERTMSG_(assoc_idx_in_pred!=string::npos, "OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!")
877  // TODO: In these cases, extend the prediction right now instead of launching an exception... or is this a bad idea??
878 
879  // Build the subset dh_dx_full_obs:
880 // dh_dx_full_obs.block(S_idxs.size() ,0, OBS_SIZE, row_len)
881 // =
882 // dh_dx_full.block (assoc_idx_in_pred*OBS_SIZE,0, OBS_SIZE, row_len);
883 
884  Eigen::Block<typename KFMatrix::Base,OBS_SIZE,VEH_SIZE> (dh_dx_full_obs, S_idxs.size(),0) = Hxs[assoc_idx_in_pred];
885  Eigen::Block<typename KFMatrix::Base,OBS_SIZE,FEAT_SIZE>(dh_dx_full_obs, S_idxs.size(), VEH_SIZE+assoc_idx_in_map*FEAT_SIZE ) = Hys[assoc_idx_in_pred];
886 
887  // S_idxs.size() is used as counter for "dh_dx_full_obs".
888  for (size_t k=0;k<OBS_SIZE;k++)
889  S_idxs.push_back(assoc_idx_in_pred*OBS_SIZE+k);
890 
891  // ytilde_i = Z[i] - all_predictions[i]
892  KFArray_OBS ytilde_i = Z[i];
893  OnSubstractObservationVectors(ytilde_i,all_predictions[predictLMidxs[assoc_idx_in_pred]]);
894  for (size_t k=0;k<OBS_SIZE;k++)
895  ytilde[ytilde_idx++] = ytilde_i[k];
896  }
897  // Extract the subset that is involved in this observation:
898  S.extractSubmatrixSymmetrical(S_idxs,S_observed);
899  }
900  else
901  { // Non-SLAM problems:
902  ASSERT_(Z.size()==1 && all_predictions.size()==1)
903  ASSERT_(dh_dx_full_obs.getRowCount()==OBS_SIZE && dh_dx_full_obs.getColCount()==VEH_SIZE)
904  ASSERT_(Hxs.size()==1)
905 
906  dh_dx_full_obs = Hxs[0]; // Was: dh_dx_full
907  KFArray_OBS ytilde_i = Z[0];
908  OnSubstractObservationVectors(ytilde_i,all_predictions[0]);
909  for (size_t k=0;k<OBS_SIZE;k++)
910  ytilde[ytilde_idx++] = ytilde_i[k];
911  // Extract the subset that is involved in this observation:
912  S_observed = S;
913  }
914 
915  // Compute the full K matrix:
916  // ------------------------------
917  m_timLogger.enter("KF:8.update stage:1.FULLKF:build K");
918 
919  K.setSize(m_pkk.getRowCount(), S_observed.getColCount() );
920 
921  // K = m_pkk * (~dh_dx) * S.inv() );
922  K.multiply_ABt(m_pkk, dh_dx_full_obs);
923 
924  // Inverse of S_observed -> S_1
925  S_observed.inv(S_1);
926  K*=S_1;
927 
928  m_timLogger.leave("KF:8.update stage:1.FULLKF:build K");
929 
930  // Use the full K matrix to update the mean:
931  if (nKF_iterations==1)
932  {
933  m_timLogger.enter("KF:8.update stage:2.FULLKF:update xkk");
934  m_xkk += K * ytilde;
935  m_timLogger.leave("KF:8.update stage:2.FULLKF:update xkk");
936  }
937  else
938  {
939  m_timLogger.enter("KF:8.update stage:2.FULLKF:iter.update xkk");
940 
941  KFVector HAx_column;
942  dh_dx_full_obs.multiply_Ab( m_xkk - xkk_0, HAx_column);
943 
944  m_xkk = xkk_0;
945  K.multiply_Ab(
946  (ytilde-HAx_column),
947  m_xkk,
948  true /* Accumulate in output */
949  );
950 
951  m_timLogger.leave("KF:8.update stage:2.FULLKF:iter.update xkk");
952  }
953 
954  // Update the covariance just at the end
955  // of iterations if we are in IKF, always in normal EKF.
956  if (IKF_iteration == (nKF_iterations-1) )
957  {
958  m_timLogger.enter("KF:8.update stage:3.FULLKF:update Pkk");
959 
960  // Use the full K matrix to update the covariance:
961  //m_pkk = (I - K*dh_dx ) * m_pkk;
962  // TODO: "Optimize this: sparsity!"
963 
964  // K * dh_dx_full_obs
965  aux_K_dh_dx.multiply(K,dh_dx_full_obs);
966 
967  // aux_K_dh_dx <-- I-aux_K_dh_dx
968  const size_t stat_len = aux_K_dh_dx.getColCount();
969  for (size_t r=0;r<stat_len;r++)
970  for (size_t c=0;c<stat_len;c++)
971  if (r==c)
972  aux_K_dh_dx.get_unsafe(r,c)=-aux_K_dh_dx.get_unsafe(r,c) + kftype(1);
973  else aux_K_dh_dx.get_unsafe(r,c)=-aux_K_dh_dx.get_unsafe(r,c);
974 
975  m_pkk.multiply_result_is_symmetric(aux_K_dh_dx, m_pkk );
976 
977  m_timLogger.leave("KF:8.update stage:3.FULLKF:update Pkk");
978  }
979  } // end for each IKF iteration
980  }
981  }
982  break;
983 
984  // --------------------------------------------------------------------
985  // - EKF 'a la' Davison: One observation element at once
986  // --------------------------------------------------------------------
987  case kfEKFAlaDavison:
988  {
989  // For each observed landmark/whole system state:
990  for (size_t obsIdx=0;obsIdx<Z.size();obsIdx++)
991  {
992  // Known & mapped landmark?
993  bool doit;
994  size_t idxInTheFilter=0;
995 
996  if (data_association.empty())
997  {
998  doit = true;
999  }
1000  else
1001  {
1002  doit = data_association[obsIdx] >= 0;
1003  if (doit)
1004  idxInTheFilter = data_association[obsIdx];
1005  }
1006 
1007  if ( doit )
1008  {
1009  m_timLogger.enter("KF:8.update stage:1.ScalarAtOnce.prepare");
1010 
1011  // Already mapped: OK
1012  const size_t idx_off = VEH_SIZE + idxInTheFilter*FEAT_SIZE; // The offset in m_xkk & Pkk.
1013 
1014  // Compute just the part of the Jacobian that we need using the current updated m_xkk:
1015  vector_KFArray_OBS pred_obs;
1016  OnObservationModel( vector_size_t(1,idxInTheFilter),pred_obs);
1017  ASSERTDEB_(pred_obs.size()==1);
1018 
1019  // ytilde = observation - prediction
1020  KFArray_OBS ytilde = Z[obsIdx];
1021  OnSubstractObservationVectors(ytilde, pred_obs[0]);
1022 
1023  // Jacobians:
1024  // dh_dx: already is (N_pred*OBS_SIZE) x (VEH_SIZE + FEAT_SIZE * N_pred )
1025  // with N_pred = |predictLMidxs|
1026 
1027  const size_t i_idx_in_preds = mrpt::utils::find_in_vector(idxInTheFilter,predictLMidxs);
1028  ASSERTMSG_(i_idx_in_preds!=string::npos, "OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!")
1029 
1030  const KFMatrix_OxV & Hx = Hxs[i_idx_in_preds];
1031  const KFMatrix_OxF & Hy = Hys[i_idx_in_preds];
1032 
1033  m_timLogger.leave("KF:8.update stage:1.ScalarAtOnce.prepare");
1034 
1035  // For each component of the observation:
1036  for (size_t j=0;j<OBS_SIZE;j++)
1037  {
1038  m_timLogger.enter("KF:8.update stage:2.ScalarAtOnce.update");
1039 
1040  // Compute the scalar S_i for each component j of the observation:
1041  // Sij = dhij_dxv Pxx dhij_dxv^t + 2 * dhij_dyi Pyix dhij_dxv + dhij_dyi Pyiyi dhij_dyi^t + R
1042  // ^^
1043  // Hx:(O*LxSv)
1044  // \----------------------/ \--------------------------/ \------------------------/ \-/
1045  // TERM 1 TERM 2 TERM 3 R
1046  //
1047  // O: Observation size (3)
1048  // L: # landmarks
1049  // Sv: Vehicle state size (6)
1050  //
1051 
1052  #if defined(_DEBUG)
1053  {
1054  // This algorithm is applicable only if the scalar component of the sensor noise are INDEPENDENT:
1055  for (size_t a=0;a<OBS_SIZE;a++)
1056  for (size_t b=0;b<OBS_SIZE;b++)
1057  if ( a!=b )
1058  if (R(a,b)!=0)
1059  THROW_EXCEPTION("This KF algorithm assumes independent noise components in the observation (matrix R). Select another KF algorithm.")
1060  }
1061  #endif
1062  // R:
1063  KFTYPE Sij = R.get_unsafe(j,j);
1064 
1065  // TERM 1:
1066  for (size_t k=0;k<VEH_SIZE;k++)
1067  {
1068  KFTYPE accum = 0;
1069  for (size_t q=0;q<VEH_SIZE;q++)
1070  accum += Hx.get_unsafe(j,q) * m_pkk.get_unsafe(q,k);
1071  Sij+= Hx.get_unsafe(j,k) * accum;
1072  }
1073 
1074  // TERM 2:
1075  KFTYPE term2=0;
1076  for (size_t k=0;k<VEH_SIZE;k++)
1077  {
1078  KFTYPE accum = 0;
1079  for (size_t q=0;q<FEAT_SIZE;q++)
1080  accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,k);
1081  term2+= Hx.get_unsafe(j,k) * accum;
1082  }
1083  Sij += 2 * term2;
1084 
1085  // TERM 3:
1086  for (size_t k=0;k<FEAT_SIZE;k++)
1087  {
1088  KFTYPE accum = 0;
1089  for (size_t q=0;q<FEAT_SIZE;q++)
1090  accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,idx_off+k);
1091  Sij+= Hy.get_unsafe(j,k) * accum;
1092  }
1093 
1094  // Compute the Kalman gain "Kij" for this observation element:
1095  // --> K = m_pkk * (~dh_dx) * S.inv() );
1096  size_t N = m_pkk.getColCount();
1097  vector<KFTYPE> Kij( N );
1098 
1099  for (size_t k=0;k<N;k++)
1100  {
1101  KFTYPE K_tmp = 0;
1102 
1103  // dhi_dxv term
1104  size_t q;
1105  for (q=0;q<VEH_SIZE;q++)
1106  K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(j,q);
1107 
1108  // dhi_dyi term
1109  for (q=0;q<FEAT_SIZE;q++)
1110  K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(j,q);
1111 
1112  Kij[k] = K_tmp / Sij;
1113  } // end for k
1114 
1115 
1116  // Update the state vector m_xkk:
1117  // x' = x + Kij * ytilde(ij)
1118  for (size_t k=0;k<N;k++)
1119  m_xkk[k] += Kij[k] * ytilde[j];
1120 
1121  // Update the covariance Pkk:
1122  // P' = P - Kij * Sij * Kij^t
1123  {
1124  for (size_t k=0;k<N;k++)
1125  {
1126  for (size_t q=k;q<N;q++) // Half matrix
1127  {
1128  m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
1129  // It is symmetric
1130  m_pkk(q,k) = m_pkk(k,q);
1131  }
1132 
1133  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
1134  if (m_pkk(k,k)<0)
1135  {
1136  m_pkk.saveToTextFile("Pkk_err.txt");
1137  mrpt::system::vectorToTextFile(Kij,"Kij.txt");
1138  ASSERT_(m_pkk(k,k)>0)
1139  }
1140  #endif
1141  }
1142  }
1143 
1144 
1145  m_timLogger.leave("KF:8.update stage:2.ScalarAtOnce.update");
1146  } // end for j
1147  } // end if mapped
1148  } // end for each observed LM.
1149  }
1150  break;
1151 
1152  // --------------------------------------------------------------------
1153  // - IKF method, processing each observation scalar secuentially:
1154  // --------------------------------------------------------------------
1155  case kfIKF: // TODO !!
1156  {
1157  THROW_EXCEPTION("IKF scalar by scalar not implemented yet.");
1158 #if 0
1159  KFMatrix h,Hx,Hy;
1160 
1161  // Just one, or several update iterations??
1162  size_t nKF_iterations = KF_options.IKF_iterations;
1163 
1164  // To avoid wasting time, if we are doing 1 iteration only, do not reserve memory for this matrix:
1165  KFMatrix *saved_Pkk=NULL;
1166  if (nKF_iterations>1)
1167  {
1168  // Create a copy of Pkk for later restoring it at the beginning of each iteration:
1169  saved_Pkk = new KFMatrix( m_pkk );
1170  }
1171 
1172  KFVector xkk_0 = m_xkk; // First state vector at the beginning of IKF
1173  KFVector xkk_next_iter = m_xkk; // for IKF only
1174 
1175  // For each IKF iteration (or 1 for EKF)
1176  for (size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
1177  {
1178  // Restore initial covariance matrix.
1179  // The updated mean is stored in m_xkk and is improved with each estimation,
1180  // and so do the Jacobians which use that improving mean.
1181  if (IKF_iteration>0)
1182  {
1183  m_pkk = *saved_Pkk;
1184  xkk_next_iter = xkk_0;
1185  }
1186 
1187  // For each observed landmark/whole system state:
1188  for (size_t obsIdx=0;obsIdx<Z.getRowCount();obsIdx++)
1189  {
1190  // Known & mapped landmark?
1191  bool doit;
1192  size_t idxInTheFilter=0;
1193 
1194  if (data_association.empty())
1195  {
1196  doit = true;
1197  }
1198  else
1199  {
1200  doit = data_association[obsIdx] >= 0;
1201  if (doit)
1202  idxInTheFilter = data_association[obsIdx];
1203  }
1204 
1205  if ( doit )
1206  {
1207  // Already mapped: OK
1208  const size_t idx_off = VEH_SIZE + idxInTheFilter *FEAT_SIZE; // The offset in m_xkk & Pkk.
1209  const size_t R_row_offset = obsIdx*OBS_SIZE; // Offset row in R
1210 
1211  // Compute just the part of the Jacobian that we need using the current updated m_xkk:
1212  KFVector ytilde; // Diff. observation - prediction
1213  OnObservationModelAndJacobians(
1214  Z,
1215  data_association,
1216  false, // Only a partial Jacobian
1217  (int)obsIdx, // Which row from Z
1218  ytilde,
1219  Hx,
1220  Hy );
1221 
1222  ASSERTDEB_(ytilde.size() == OBS_SIZE )
1223  ASSERTDEB_(Hx.getRowCount() == OBS_SIZE )
1224  ASSERTDEB_(Hx.getColCount() == VEH_SIZE )
1225 
1226  if (FEAT_SIZE>0)
1227  {
1228  ASSERTDEB_(Hy.getRowCount() == OBS_SIZE )
1229  ASSERTDEB_(Hy.getColCount() == FEAT_SIZE )
1230  }
1231 
1232  // Compute the OxO matrix S_i for each observation:
1233  // Si = TERM1 + TERM2 + TERM2^t + TERM3 + R
1234  //
1235  // TERM1: dhi_dxv Pxx dhi_dxv^t
1236  // ^^
1237  // Hx:(OxV)
1238  //
1239  // TERM2: dhi_dyi Pyix dhi_dxv
1240  // ^^
1241  // Hy:(OxF)
1242  //
1243  // TERM3: dhi_dyi Pyiyi dhi_dyi^t
1244  //
1245  // i: obsIdx
1246  // O: Observation size
1247  // F: Feature size
1248  // V: Vehicle state size
1249  //
1250 
1251  // R:
1252  KFMatrix Si(OBS_SIZE,OBS_SIZE);
1253  R.extractMatrix(R_row_offset,0, Si);
1254 
1255  size_t k;
1256  KFMatrix term(OBS_SIZE,OBS_SIZE);
1257 
1258  // TERM1: dhi_dxv Pxx dhi_dxv^t
1259  Hx.multiply_HCHt(
1260  m_pkk, // The cov. matrix
1261  Si, // The output
1262  true, // Yes, submatrix of m_pkk only
1263  0, // Offset in m_pkk
1264  true // Yes, accum results in output.
1265  );
1266 
1267  // TERM2: dhi_dyi Pyix dhi_dxv
1268  // + its transpose:
1269  KFMatrix Pyix( FEAT_SIZE, VEH_SIZE );
1270  m_pkk.extractMatrix(idx_off,0, Pyix); // Extract cross cov. to Pyix
1271 
1272  term.multiply_ABCt( Hy, Pyix, Hx ); //term = Hy * Pyix * ~Hx;
1273  Si.add_AAt( term ); // Si += A + ~A
1274 
1275  // TERM3: dhi_dyi Pyiyi dhi_dyi^t
1276  Hy.multiply_HCHt(
1277  m_pkk, // The cov. matrix
1278  Si, // The output
1279  true, // Yes, submatrix of m_pkk only
1280  idx_off, // Offset in m_pkk
1281  true // Yes, accum results in output.
1282  );
1283 
1284  // Compute the inverse of Si:
1285  KFMatrix Si_1(OBS_SIZE,OBS_SIZE);
1286 
1287  // Compute the Kalman gain "Ki" for this i'th observation:
1288  // --> Ki = m_pkk * (~dh_dx) * S.inv();
1289  size_t N = m_pkk.getColCount();
1290 
1291  KFMatrix Ki( N, OBS_SIZE );
1292 
1293  for (k=0;k<N;k++) // for each row of K
1294  {
1295  size_t q;
1296 
1297  for (size_t c=0;c<OBS_SIZE;c++) // for each column of K
1298  {
1299  KFTYPE K_tmp = 0;
1300 
1301  // dhi_dxv term
1302  for (q=0;q<VEH_SIZE;q++)
1303  K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(c,q);
1304 
1305  // dhi_dyi term
1306  for (q=0;q<FEAT_SIZE;q++)
1307  K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(c,q);
1308 
1309  Ki.set_unsafe(k,c, K_tmp);
1310  } // end for c
1311  } // end for k
1312 
1313  Ki.multiply(Ki, Si.inv() ); // K = (...) * S^-1
1314 
1315 
1316  // Update the state vector m_xkk:
1317  if (nKF_iterations==1)
1318  {
1319  // EKF:
1320  // x' = x + Kij * ytilde(ij)
1321  for (k=0;k<N;k++)
1322  for (size_t q=0;q<OBS_SIZE;q++)
1323  m_xkk[k] += Ki.get_unsafe(k,q) * ytilde[q];
1324  }
1325  else
1326  {
1327  // IKF:
1328  mrpt::dynamicsize_vector<KFTYPE> HAx(OBS_SIZE);
1329  size_t o,q;
1330  // HAx = H*(x0-xi)
1331  for (o=0;o<OBS_SIZE;o++)
1332  {
1333  KFTYPE tmp = 0;
1334  for (q=0;q<VEH_SIZE;q++)
1335  tmp += Hx.get_unsafe(o,q) * (xkk_0[q] - m_xkk[q]);
1336 
1337  for (q=0;q<FEAT_SIZE;q++)
1338  tmp += Hy.get_unsafe(o,q) * (xkk_0[idx_off+q] - m_xkk[idx_off+q]);
1339 
1340  HAx[o] = tmp;
1341  }
1342 
1343  // Compute only once (ytilde[j] - HAx)
1344  for (o=0;o<OBS_SIZE;o++)
1345  HAx[o] = ytilde[o] - HAx[o];
1346 
1347  // x' = x_0 + Kij * ( ytilde(ij) - H*(x0-xi)) -> xi: i=this iteration
1348  // xkk_next_iter is initialized to xkk_0 at each IKF iteration.
1349  for (k=0;k<N;k++)
1350  {
1351  KFTYPE tmp = xkk_next_iter[k];
1352  //m_xkk[k] = xkk_0[k] + Kij[k] * (ytilde[j] - HAx );
1353  for (o=0;o<OBS_SIZE;o++)
1354  tmp += Ki.get_unsafe(k,o) * HAx[o];
1355 
1356  xkk_next_iter[k] = tmp;
1357  }
1358  }
1359 
1360  // Update the covariance Pkk:
1361  // P' = P - Kij * Sij * Kij^t
1362  //if (IKF_iteration==(nKF_iterations-1)) // Just for the last IKF iteration
1363  {
1364  // m_pkk -= Ki * Si * ~Ki;
1365  Ki.multiplyByMatrixAndByTransposeNonSymmetric(
1366  Si,
1367  m_pkk, // Output
1368  true, // Accumulate
1369  true); // Substract instead of sum.
1370 
1371  m_pkk.force_symmetry();
1372 
1373  /* for (k=0;k<N;k++)
1374  {
1375  for (size_t q=k;q<N;q++) // Half matrix
1376  {
1377  m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
1378  // It is symmetric
1379  m_pkk(q,k) = m_pkk(k,q);
1380  }
1381 
1382  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
1383  if (m_pkk(k,k)<0)
1384  {
1385  m_pkk.saveToTextFile("Pkk_err.txt");
1386  mrpt::system::vectorToTextFile(Kij,"Kij.txt");
1387  ASSERT_(m_pkk(k,k)>0)
1388  }
1389  #endif
1390  }
1391  */
1392  }
1393 
1394  } // end if doit
1395 
1396  } // end for each observed LM.
1397 
1398  // Now, save the new iterated mean:
1399  if (nKF_iterations>1)
1400  {
1401  #if 0
1402  cout << "IKF iter: " << IKF_iteration << " -> " << xkk_next_iter << endl;
1403  #endif
1404  m_xkk = xkk_next_iter;
1405  }
1406 
1407  } // end for each IKF_iteration
1408 
1409  // Copy of pkk not required anymore:
1410  if (saved_Pkk) delete saved_Pkk;
1411 
1412 #endif
1413  }
1414  break;
1415 
1416  default:
1417  THROW_EXCEPTION("Invalid value of options.KF_method");
1418  } // end switch method
1419 
1420  }
1421 
1422  const double tim_update = m_timLogger.leave("KF:8.update stage");
1423 
1424  m_timLogger.enter("KF:9.OnNormalizeStateVector");
1425  OnNormalizeStateVector();
1426  m_timLogger.leave("KF:9.OnNormalizeStateVector");
1427 
1428  // =============================================================
1429  // 8. INTRODUCE NEW LANDMARKS
1430  // =============================================================
1431  if (!data_association.empty())
1432  {
1433  m_timLogger.enter("KF:A.add new landmarks");
1434  detail::CRunOneKalmanIteration_addNewLandmarks()(*this, Z, data_association,R);
1435  m_timLogger.leave("KF:A.add new landmarks");
1436  } // end if data_association!=empty
1437 
1438  // Post iteration user code:
1439  m_timLogger.enter("KF:B.OnPostIteration");
1440  OnPostIteration();
1441  m_timLogger.leave("KF:B.OnPostIteration");
1442 
1443  m_timLogger.leave("KF:complete_step");
1444 
1445  if (KF_options.verbose)
1446  {
1447  printf_debug("[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: %.2fms\n",
1448  static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
1449  1e3*tim_pred,
1450  1e3*tim_pred_obs,
1451  1e3*tim_obs_DA,
1452  1e3*tim_update
1453  );
1454  }
1455  MRPT_END
1456 
1457  } // End of "runOneKalmanIteration"
1458 
1459 
1460 
1461  private:
1462  mutable bool m_user_didnt_implement_jacobian;
1463 
1464  /** Auxiliary functions for Jacobian numeric estimation */
1465  static void KF_aux_estimate_trans_jacobian(
1466  const KFArray_VEH &x,
1467  const std::pair<KFCLASS*,KFArray_ACT> &dat,
1468  KFArray_VEH &out_x)
1469  {
1470  bool dumm;
1471  out_x=x;
1472  dat.first->OnTransitionModel(dat.second,out_x, dumm);
1473  }
1474  static void KF_aux_estimate_obs_Hx_jacobian(
1475  const KFArray_VEH &x,
1476  const std::pair<KFCLASS*,size_t> &dat,
1477  KFArray_OBS &out_x)
1478  {
1479  vector_size_t idxs_to_predict(1,dat.second);
1480  vector_KFArray_OBS prediction;
1481  // Overwrite (temporarily!) the affected part of the state vector:
1482  ::memcpy(&dat.first->m_xkk[0],&x[0],sizeof(x[0])*VEH_SIZE);
1483  dat.first->OnObservationModel(idxs_to_predict,prediction);
1484  ASSERTDEB_(prediction.size()==1);
1485  out_x=prediction[0];
1486  }
1487  static void KF_aux_estimate_obs_Hy_jacobian(
1488  const KFArray_FEAT &x,
1489  const std::pair<KFCLASS*,size_t> &dat,
1490  KFArray_OBS &out_x)
1491  {
1492  vector_size_t idxs_to_predict(1,dat.second);
1493  vector_KFArray_OBS prediction;
1494  const size_t lm_idx_in_statevector = VEH_SIZE+FEAT_SIZE*dat.second;
1495  // Overwrite (temporarily!) the affected part of the state vector:
1496  ::memcpy(&dat.first->m_xkk[lm_idx_in_statevector],&x[0],sizeof(x[0])*FEAT_SIZE);
1497  dat.first->OnObservationModel(idxs_to_predict,prediction);
1498  ASSERTDEB_(prediction.size()==1);
1499  out_x=prediction[0];
1500  }
1501 
1504 
1505  }; // end class
1506 
1507  namespace detail
1508  {
1510  {
1511  // generic version for SLAM. There is a speciation below for NON-SLAM problems.
1512  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1516  const vector_int &data_association,
1518  )
1519  {
1521 
1522  for (size_t idxObs=0;idxObs<Z.size();idxObs++)
1523  {
1524  // Is already in the map?
1525  if ( data_association[idxObs] < 0 ) // Not in the map yet!
1526  {
1527  obj.m_timLogger.enter("KF:9.create new LMs");
1528  // Add it:
1529 
1530  // Append to map of IDs <-> position in the state vector:
1531  ASSERTDEB_(FEAT_SIZE>0)
1532  ASSERTDEB_( 0 == ((obj.m_xkk.size() - VEH_SIZE) % FEAT_SIZE) ) // Sanity test
1533 
1534  const size_t newIndexInMap = (obj.m_xkk.size() - VEH_SIZE) / FEAT_SIZE;
1535 
1536  // Inverse sensor model:
1537  typename KF::KFArray_FEAT yn;
1538  typename KF::KFMatrix_FxV dyn_dxv;
1539  typename KF::KFMatrix_FxO dyn_dhn;
1540  typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1541  bool use_dyn_dhn_jacobian=true;
1542 
1543  // Compute the inv. sensor model and its Jacobians:
1544  obj.OnInverseObservationModel(
1545  Z[idxObs],
1546  yn,
1547  dyn_dxv,
1548  dyn_dhn,
1549  dyn_dhn_R_dyn_dhnT,
1550  use_dyn_dhn_jacobian );
1551 
1552  // And let the application do any special handling of adding a new feature to the map:
1553  obj.OnNewLandmarkAddedToMap(
1554  idxObs,
1555  newIndexInMap
1556  );
1557 
1558  ASSERTDEB_( yn.size() == FEAT_SIZE )
1559 
1560  // Append to m_xkk:
1561  size_t q;
1562  size_t idx = obj.m_xkk.size();
1563  obj.m_xkk.resize( obj.m_xkk.size() + FEAT_SIZE );
1564 
1565  for (q=0;q<FEAT_SIZE;q++)
1566  obj.m_xkk[idx+q] = yn[q];
1567 
1568  // --------------------
1569  // Append to Pkk:
1570  // --------------------
1571  ASSERTDEB_( obj.m_pkk.getColCount()==idx && obj.m_pkk.getRowCount()==idx );
1572 
1573  obj.m_pkk.setSize( idx+FEAT_SIZE,idx+FEAT_SIZE );
1574 
1575  // Fill the Pxyn term:
1576  // --------------------
1577  typename KF::KFMatrix_VxV Pxx;
1578  obj.m_pkk.extractMatrix(0,0,Pxx);
1579  typename KF::KFMatrix_FxV Pxyn; // Pxyn = dyn_dxv * Pxx
1580  Pxyn.multiply( dyn_dxv, Pxx );
1581 
1582  obj.m_pkk.insertMatrix( idx,0, Pxyn );
1583  obj.m_pkk.insertMatrixTranspose( 0,idx, Pxyn );
1584 
1585  // Fill the Pyiyn terms:
1586  // --------------------
1587  const size_t nLMs = (idx-VEH_SIZE)/FEAT_SIZE; // Number of previous landmarks:
1588  for (q=0;q<nLMs;q++)
1589  {
1590  typename KF::KFMatrix_VxF P_x_yq(UNINITIALIZED_MATRIX);
1591  obj.m_pkk.extractMatrix(0,VEH_SIZE+q*FEAT_SIZE,P_x_yq) ;
1592 
1593  typename KF::KFMatrix_FxF P_cross(UNINITIALIZED_MATRIX);
1594  P_cross.multiply(dyn_dxv, P_x_yq );
1595 
1596  obj.m_pkk.insertMatrix(idx,VEH_SIZE+q*FEAT_SIZE, P_cross );
1597  obj.m_pkk.insertMatrixTranspose(VEH_SIZE+q*FEAT_SIZE,idx, P_cross );
1598  } // end each previous LM(q)
1599 
1600  // Fill the Pynyn term:
1601  // P_yn_yn = (dyn_dxv * Pxx * ~dyn_dxv) + (dyn_dhn * R * ~dyn_dhn);
1602  // --------------------
1603  typename KF::KFMatrix_FxF P_yn_yn(UNINITIALIZED_MATRIX);
1604  dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
1605  if (use_dyn_dhn_jacobian)
1606  dyn_dhn.multiply_HCHt(R, P_yn_yn, true); // Accumulate in P_yn_yn
1607  else P_yn_yn+=dyn_dhn_R_dyn_dhnT;
1608 
1609  obj.m_pkk.insertMatrix(idx,idx, P_yn_yn );
1610 
1611  obj.m_timLogger.leave("KF:9.create new LMs");
1612  }
1613  }
1614  }
1615 
1616  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1620  const vector_int &data_association,
1622  )
1623  {
1624  // Do nothing: this is NOT a SLAM problem.
1625  }
1626 
1627  }; // end runOneKalmanIteration_addNewLandmarks
1628 
1629  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1631  {
1632  return (obj.getStateVectorLength()-VEH_SIZE)/FEAT_SIZE;
1633  }
1634  // Specialization for FEAT_SIZE=0
1635  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1637  {
1638  return 0;
1639  }
1640 
1641  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1643  {
1644  return (obj.getStateVectorLength()==VEH_SIZE);
1645  }
1646  // Specialization for FEAT_SIZE=0
1647  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1649  {
1650  return true;
1651  }
1652 
1653  } // end namespace "detail"
1654 
1655  } // end namespace
1656 
1657  // Specializations MUST occur at the same namespace:
1658  namespace utils
1659  {
1660  template <>
1662  {
1664  static void fill(bimap<enum_t,std::string> &m_map)
1665  {
1666  m_map.insert(bayes::kfEKFNaive, "kfEKFNaive");
1667  m_map.insert(bayes::kfEKFAlaDavison, "kfEKFAlaDavison");
1668  m_map.insert(bayes::kfIKFFull, "kfIKFFull");
1669  m_map.insert(bayes::kfIKF, "kfIKF");
1670  }
1671  };
1672  } // End of namespace
1673 
1674 } // end namespace
1675 
1676 #endif
bool use_analytic_observation_jacobian
(default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estima...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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)
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".
virtual void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
virtual void OnInverseObservationModel(const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn, 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...
bool enable_profiler
If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLo...
#define THROW_EXCEPTION(msg)
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArray.h:305
CMatrixFixedNumeric< KFTYPE, VEH_SIZE, FEAT_SIZE > KFMatrix_VxF
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).
STL namespace.
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
virtual void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
KFMatrix m_pkk
The system full covariance matrix.
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...
void operator()(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)
void enable(bool enabled=true)
Definition: CTimeLogger.h:70
This class allows loading and storing values and vectors of different types from a configuration text...
std::vector< size_t > vector_size_t
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...
CArrayNumeric< KFTYPE, OBS_SIZE > KFArray_OBS
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:36
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.
This base provides a set of functions for maths stuff.
Definition: CArray.h:19
void operator()(CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE > &obj, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE >::vector_KFArray_OBS &Z, const vector_int &data_association, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE >::KFMatrix_OxO &R)
#define MRPT_END
std::vector< size_t > vector_size_t
mrpt::dynamicsize_vector< KFTYPE > KFVector
A helper class that can convert an enum value into its textual representation, and viceversa...
Definition: TEnumType.h:32
bool verbose
If set to true timing and other information will be dumped during the execution (default=false) ...
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)...
CMatrixFixedNumeric< KFTYPE, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
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:29
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...
CArrayNumeric< KFTYPE, VEH_SIZE > KFArray_VEH
EIGEN_STRONG_INLINE MatrixBase< Derived > & Abs()
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...
virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
Generic options for the Kalman Filter algorithm in itself.
CMatrixFixedNumeric< KFTYPE, VEH_SIZE, OBS_SIZE > KFMatrix_VxO
CMatrixFixedNumeric< KFTYPE, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
void estimateJacobian(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
KFTYPE kftype
The numeric type used in the Kalman Filter (default=double)
#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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CArrayNumeric< KFTYPE, ACT_SIZE > KFArray_ACT
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
EIGEN_STRONG_INLINE Scalar sumAll() const
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE > &obj)
virtual void OnInverseObservationModel(const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const
If applicable to the given problem, this method implements the inverse observation model needed to ex...
void 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.
std::vector< int32_t > vector_int
CMatrixFixedNumeric< KFTYPE, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
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...
CMatrixFixedNumeric< KFTYPE, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
CMatrixTemplateNumeric< KFTYPE > KFMatrix
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
#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:34
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:81
bool BASE_IMPEXP vectorToTextFile(const std::vector< float > &vec, const std::string &fileName, bool append=false, bool byRows=false)
A useful function for debuging, which saves a std::vector into a text file (compat.
CArrayNumeric< KFTYPE, FEAT_SIZE > KFArray_FEAT
TKFMethod method
The method to employ (default: kfEKFNaive)
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, 0, ACT_SIZE, KFTYPE > &obj)
CMatrixFixedNumeric< KFTYPE, ACT_SIZE, ACT_SIZE > KFMatrix_AxA
void dumpToTextStream(CStream &out) const
This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:70
size_t find_in_vector(const T &value, const CONTAINER &vect)
Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:76
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...
CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, FEAT_SIZE > KFMatrix_FxF
#define ASSERTMSG_(f, __ERROR_MSG)
KFVector m_xkk
The system state vector.
This base class provides a common printf-like method to send debug information to std::cout...
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...
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
CMatrixFixedNumeric< KFTYPE, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo