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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013