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