Main MRPT website > C++ reference for MRPT 1.9.9
CKalmanFilterCapable_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef CKalmanFilterCapable_IMPL_H
10 #define CKalmanFilterCapable_IMPL_H
11 
12 #ifndef CKalmanFilterCapable_H
13 #error Include this file only from 'CKalmanFilterCapable.h'
14 #endif
15 
17 
18 namespace mrpt
19 {
20 namespace bayes
21 {
22 // The main entry point in the Kalman Filter class:
23 template <
24  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
25  typename KFTYPE>
26 void CKalmanFilterCapable<
27  VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::runOneKalmanIteration()
28 {
29  using namespace std;
31 
32  m_timLogger.enable(KF_options.enable_profiler);
33  m_timLogger.enter("KF:complete_step");
34 
35  ASSERT_(int(m_xkk.size()) == m_pkk.cols());
36  ASSERT_(size_t(m_xkk.size()) >= VEH_SIZE);
37  // =============================================================
38  // 1. CREATE ACTION MATRIX u FROM ODOMETRY
39  // =============================================================
40  KFArray_ACT u;
41 
42  m_timLogger.enter("KF:1.OnGetAction");
43  OnGetAction(u);
44  m_timLogger.leave("KF:1.OnGetAction");
45 
46  // Sanity check:
47  if (FEAT_SIZE)
48  {
49  ASSERTDEB_(
50  (((m_xkk.size() - VEH_SIZE) / FEAT_SIZE) * FEAT_SIZE) ==
51  (m_xkk.size() - VEH_SIZE));
52  }
53 
54  // =============================================================
55  // 2. PREDICTION OF NEW POSE xv_{k+1|k}
56  // =============================================================
57  m_timLogger.enter("KF:2.prediction stage");
58 
59  const size_t N_map = getNumberOfLandmarksInTheMap();
60 
61  KFArray_VEH xv(&m_xkk[0]); // Vehicle pose
62 
63  bool skipPrediction = false; // Whether to skip the prediction step (in
64  // SLAM this is desired for the first
65  // iteration...)
66 
67  // Update mean: xv will have the updated pose until we update it in the
68  // filterState later.
69  // This is to maintain a copy of the last robot pose in the state vector,
70  // required for the Jacobian computation.
71  OnTransitionModel(u, xv, skipPrediction);
72 
73  if (!skipPrediction)
74  {
75  // =============================================================
76  // 3. PREDICTION OF COVARIANCE P_{k+1|k}
77  // =============================================================
78  // First, we compute de Jacobian fv_by_xv (derivative of f_vehicle wrt
79  // x_vehicle):
80  KFMatrix_VxV dfv_dxv;
81 
82  // Try closed-form Jacobian first:
83  m_user_didnt_implement_jacobian = false; // Set to true by the default
84  // method if not reimplemented
85  // in base class.
86  if (KF_options.use_analytic_transition_jacobian)
87  OnTransitionJacobian(dfv_dxv);
88 
89  if (m_user_didnt_implement_jacobian ||
90  !KF_options.use_analytic_transition_jacobian ||
91  KF_options.debug_verify_analytic_jacobians)
92  { // Numeric approximation:
93  KFArray_VEH xkk_vehicle(
94  &m_xkk[0]); // A copy of the vehicle part of the state vector.
95  KFArray_VEH xkk_veh_increments;
96  OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
97 
99  xkk_vehicle,
100  std::function<void(
101  const KFArray_VEH& x,
102  const std::pair<KFCLASS*, KFArray_ACT>& dat,
103  KFArray_VEH& out_x)>(&KF_aux_estimate_trans_jacobian),
104  xkk_veh_increments, std::pair<KFCLASS*, KFArray_ACT>(this, u),
105  dfv_dxv);
106 
107  if (KF_options.debug_verify_analytic_jacobians)
108  {
110  OnTransitionJacobian(dfv_dxv_gt);
111  if ((dfv_dxv - dfv_dxv_gt).array().abs().sum() >
112  KF_options.debug_verify_analytic_jacobians_threshold)
113  {
114  std::cerr << "[KalmanFilter] ERROR: User analytical "
115  "transition Jacobians are wrong: \n"
116  << " Real dfv_dxv: \n"
117  << dfv_dxv << "\n Analytical dfv_dxv:\n"
118  << dfv_dxv_gt << "Diff:\n"
119  << (dfv_dxv - dfv_dxv_gt) << "\n";
121  "ERROR: User analytical transition Jacobians are wrong "
122  "(More details dumped to cerr)")
123  }
124  }
125  }
126 
127  // Q is the process noise covariance matrix, is associated to the robot
128  // movement and is necesary to calculate the prediction P(k+1|k)
129  KFMatrix_VxV Q;
130  OnTransitionNoise(Q);
131 
132  // ====================================
133  // 3.1: Pxx submatrix
134  // ====================================
135  // Replace old covariance:
136  Eigen::Block<typename KFMatrix::Base, VEH_SIZE, VEH_SIZE>(m_pkk, 0, 0) =
137  Q + dfv_dxv *
138  Eigen::Block<typename KFMatrix::Base, VEH_SIZE, VEH_SIZE>(
139  m_pkk, 0, 0) *
140  dfv_dxv.transpose();
141 
142  // ====================================
143  // 3.2: All Pxy_i
144  // ====================================
145  // Now, update the cov. of landmarks, if any:
146  KFMatrix_VxF aux;
147  for (size_t i = 0; i < N_map; i++)
148  {
149  aux = dfv_dxv *
150  Eigen::Block<typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>(
151  m_pkk, 0, VEH_SIZE + i * FEAT_SIZE);
152 
153  Eigen::Block<typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>(
154  m_pkk, 0, VEH_SIZE + i * FEAT_SIZE) = aux;
155  Eigen::Block<typename KFMatrix::Base, FEAT_SIZE, VEH_SIZE>(
156  m_pkk, VEH_SIZE + i * FEAT_SIZE, 0) = aux.transpose();
157  }
158 
159  // =============================================================
160  // 4. NOW WE CAN OVERWRITE THE NEW STATE VECTOR
161  // =============================================================
162  for (size_t i = 0; i < VEH_SIZE; i++) m_xkk[i] = xv[i];
163 
164  // Normalize, if neccesary.
165  OnNormalizeStateVector();
166 
167  } // end if (!skipPrediction)
168 
169  const double tim_pred = m_timLogger.leave("KF:2.prediction stage");
170 
171  // =============================================================
172  // 5. PREDICTION OF OBSERVATIONS AND COMPUTE JACOBIANS
173  // =============================================================
174  m_timLogger.enter("KF:3.predict all obs");
175 
176  KFMatrix_OxO R; // Sensor uncertainty (covariance matrix): R
177  OnGetObservationNoise(R);
178 
179  // Predict the observations for all the map LMs, so the user
180  // can decide if their covariances (more costly) must be computed as well:
181  all_predictions.resize(N_map);
182  OnObservationModel(
183  mrpt::math::sequenceStdVec<size_t, 1>(0, N_map), all_predictions);
184 
185  const double tim_pred_obs = m_timLogger.leave("KF:3.predict all obs");
186 
187  m_timLogger.enter("KF:4.decide pred obs");
188 
189  // Decide if some of the covariances shouldn't be predicted.
190  predictLMidxs.clear();
191  if (FEAT_SIZE == 0)
192  // In non-SLAM problems, just do one prediction, for the entire system
193  // state:
194  predictLMidxs.assign(1, 0);
195  else
196  // On normal SLAM problems:
197  OnPreComputingPredictions(all_predictions, predictLMidxs);
198 
199  m_timLogger.leave("KF:4.decide pred obs");
200 
201  // =============================================================
202  // 6. COMPUTE INNOVATION MATRIX "S"
203  // =============================================================
204  // Do the prediction of the observation covariances:
205  // Compute S: S = H P ~H + R
206  //
207  // Build a big dh_dx Jacobian composed of the small block Jacobians.
208  // , but: it's actually a subset of the full Jacobian, since the
209  // non-predicted
210  // features do not appear.
211  //
212  // dh_dx: O*M x V+M*F
213  // S: O*M x O*M
214  // M = |predictLMidxs|
215  // O=size of each observation.
216  // F=size of features in the map
217  //
218  // Updated: Now we only keep the non-zero blocks of that Jacobian,
219  // in the vectors Hxs[] and Hys[].
220  //
221 
222  m_timLogger.enter("KF:5.build Jacobians");
223 
224  size_t N_pred =
225  FEAT_SIZE == 0
226  ? 1 /* In non-SLAM problems, there'll be only 1 fixed observation */
227  : predictLMidxs.size();
228 
229  std::vector<int>
230  data_association; // -1: New map feature.>=0: Indexes in the
231  // state vector
232 
233  // The next loop will only do more than one iteration if the heuristic in
234  // OnPreComputingPredictions() fails,
235  // which will be detected by the addition of extra landmarks to predict
236  // into "missing_predictions_to_add"
237  std::vector<size_t> missing_predictions_to_add;
238 
239  Hxs.resize(N_pred); // Lists of partial Jacobians
240  Hys.resize(N_pred);
241 
242  size_t first_new_pred = 0; // This will be >0 only if we perform multiple
243  // loops due to failures in the prediction
244  // heuristic.
245 
246  do
247  {
248  if (!missing_predictions_to_add.empty())
249  {
250  const size_t nNew = missing_predictions_to_add.size();
252  "[KF] *Performance Warning*: "
253  << nNew
254  << " LMs were not correctly predicted by "
255  "OnPreComputingPredictions().");
256 
257  ASSERTDEB_(FEAT_SIZE != 0);
258  for (size_t j = 0; j < nNew; j++)
259  predictLMidxs.push_back(missing_predictions_to_add[j]);
260 
261  N_pred = predictLMidxs.size();
262  missing_predictions_to_add.clear();
263  }
264 
265  Hxs.resize(N_pred); // Append new entries, if needed.
266  Hys.resize(N_pred);
267 
268  for (size_t i = first_new_pred; i < N_pred; ++i)
269  {
270  const size_t lm_idx = FEAT_SIZE == 0 ? 0 : predictLMidxs[i];
271  KFMatrix_OxV& Hx = Hxs[i];
272  KFMatrix_OxF& Hy = Hys[i];
273 
274  // Try the analitic Jacobian first:
275  m_user_didnt_implement_jacobian =
276  false; // Set to true by the default method if not
277  // reimplemented in base class.
278  if (KF_options.use_analytic_observation_jacobian)
279  OnObservationJacobians(lm_idx, Hx, Hy);
280 
281  if (m_user_didnt_implement_jacobian ||
282  !KF_options.use_analytic_observation_jacobian ||
283  KF_options.debug_verify_analytic_jacobians)
284  { // Numeric approximation:
285  const size_t lm_idx_in_statevector =
286  VEH_SIZE + lm_idx * FEAT_SIZE;
287 
288  const KFArray_VEH x_vehicle(&m_xkk[0]);
289  const KFArray_FEAT x_feat(&m_xkk[lm_idx_in_statevector]);
290 
291  KFArray_VEH xkk_veh_increments;
292  KFArray_FEAT feat_increments;
293  OnObservationJacobiansNumericGetIncrements(
294  xkk_veh_increments, feat_increments);
295 
297  x_vehicle,
298  std::function<void(
299  const KFArray_VEH& x,
300  const std::pair<KFCLASS*, size_t>& dat,
301  KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hx_jacobian),
302  xkk_veh_increments,
303  std::pair<KFCLASS*, size_t>(this, lm_idx), Hx);
304  // The state vector was temporarily modified by
305  // KF_aux_estimate_*, restore it:
306  ::memcpy(&m_xkk[0], &x_vehicle[0], sizeof(m_xkk[0]) * VEH_SIZE);
307 
309  x_feat,
310  std::function<void(
311  const KFArray_FEAT& x,
312  const std::pair<KFCLASS*, size_t>& dat,
313  KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hy_jacobian),
314  feat_increments, std::pair<KFCLASS*, size_t>(this, lm_idx),
315  Hy);
316  // The state vector was temporarily modified by
317  // KF_aux_estimate_*, restore it:
318  ::memcpy(
319  &m_xkk[lm_idx_in_statevector], &x_feat[0],
320  sizeof(m_xkk[0]) * FEAT_SIZE);
321 
322  if (KF_options.debug_verify_analytic_jacobians)
323  {
326  OnObservationJacobians(lm_idx, Hx_gt, Hy_gt);
327  if ((Hx - Hx_gt).array().abs().sum() >
328  KF_options.debug_verify_analytic_jacobians_threshold)
329  {
330  std::cerr << "[KalmanFilter] ERROR: User analytical "
331  "observation Hx Jacobians are wrong: \n"
332  << " Real Hx: \n"
333  << Hx << "\n Analytical Hx:\n"
334  << Hx_gt << "Diff:\n"
335  << Hx - Hx_gt << "\n";
337  "ERROR: User analytical observation Hx Jacobians "
338  "are wrong (More details dumped to cerr)")
339  }
340  if ((Hy - Hy_gt).array().abs().sum() >
341  KF_options.debug_verify_analytic_jacobians_threshold)
342  {
343  std::cerr << "[KalmanFilter] ERROR: User analytical "
344  "observation Hy Jacobians are wrong: \n"
345  << " Real Hy: \n"
346  << Hy << "\n Analytical Hx:\n"
347  << Hy_gt << "Diff:\n"
348  << Hy - Hy_gt << "\n";
350  "ERROR: User analytical observation Hy Jacobians "
351  "are wrong (More details dumped to cerr)")
352  }
353  }
354  }
355  }
356  m_timLogger.leave("KF:5.build Jacobians");
357 
358  m_timLogger.enter("KF:6.build S");
359 
360  // Compute S: S = H P ~H + R (R will be added below)
361  // exploiting the sparsity of H:
362  // Each block in S is:
363  // Sij =
364  // ------------------------------------------
365  S.setSize(N_pred * OBS_SIZE, N_pred * OBS_SIZE);
366 
367  if (FEAT_SIZE > 0)
368  { // SLAM-like problem:
369  const Eigen::Block<
370  const typename KFMatrix::Base, VEH_SIZE, VEH_SIZE>
371  Px(m_pkk, 0, 0); // Covariance of the vehicle pose
372 
373  for (size_t i = 0; i < N_pred; ++i)
374  {
375  const size_t lm_idx_i = predictLMidxs[i];
376  const Eigen::Block<
377  const typename KFMatrix::Base, FEAT_SIZE, VEH_SIZE>
378  Pxyi_t(
379  m_pkk, VEH_SIZE + lm_idx_i * FEAT_SIZE, 0); // Pxyi^t
380 
381  // Only do j>=i (upper triangle), since S is symmetric:
382  for (size_t j = i; j < N_pred; ++j)
383  {
384  const size_t lm_idx_j = predictLMidxs[j];
385  // Sij block:
386  Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>
387  Sij(S, OBS_SIZE * i, OBS_SIZE * j);
388 
389  const Eigen::Block<
390  const typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>
391  Pxyj(m_pkk, 0, VEH_SIZE + lm_idx_j * FEAT_SIZE);
392  const Eigen::Block<
393  const typename KFMatrix::Base, FEAT_SIZE, FEAT_SIZE>
394  Pyiyj(
395  m_pkk, VEH_SIZE + lm_idx_i * FEAT_SIZE,
396  VEH_SIZE + lm_idx_j * FEAT_SIZE);
397 
398  Sij = Hxs[i] * Px * Hxs[j].transpose() +
399  Hys[i] * Pxyi_t * Hxs[j].transpose() +
400  Hxs[i] * Pxyj * Hys[j].transpose() +
401  Hys[i] * Pyiyj * Hys[j].transpose();
402 
403  // Copy transposed to the symmetric lower-triangular part:
404  if (i != j)
405  Eigen::Block<
406  typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(
407  S, OBS_SIZE * j, OBS_SIZE * i) = Sij.transpose();
408  }
409 
410  // Sum the "R" term to the diagonal blocks:
411  const size_t obs_idx_off = i * OBS_SIZE;
412  Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(
413  S, obs_idx_off, obs_idx_off) += R;
414  }
415  }
416  else
417  { // Not SLAM-like problem: simply S=H*Pkk*H^t + R
418  ASSERTDEB_(N_pred == 1);
419  ASSERTDEB_(S.cols() == OBS_SIZE);
420 
421  S = Hxs[0] * m_pkk * Hxs[0].transpose() + R;
422  }
423 
424  m_timLogger.leave("KF:6.build S");
425 
426  Z.clear(); // Each entry is one observation:
427 
428  m_timLogger.enter("KF:7.get obs & DA");
429 
430  // Get observations and do data-association:
431  OnGetObservationsAndDataAssociation(
432  Z, data_association, // Out
433  all_predictions, S, predictLMidxs, R // In
434  );
435  ASSERTDEB_(
436  data_association.size() == Z.size() ||
437  (data_association.empty() && FEAT_SIZE == 0));
438 
439  // Check if an observation hasn't been predicted in
440  // OnPreComputingPredictions() but has been actually
441  // observed. This may imply an error in the heuristic of
442  // OnPreComputingPredictions(), and forces us
443  // to rebuild the matrices
444  missing_predictions_to_add.clear();
445  if (FEAT_SIZE != 0)
446  {
447  for (size_t i = 0; i < data_association.size(); ++i)
448  {
449  if (data_association[i] < 0) continue;
450  const size_t assoc_idx_in_map =
451  static_cast<size_t>(data_association[i]);
452  const size_t assoc_idx_in_pred =
454  assoc_idx_in_map, predictLMidxs);
455  if (assoc_idx_in_pred == std::string::npos)
456  missing_predictions_to_add.push_back(assoc_idx_in_map);
457  }
458  }
459 
460  first_new_pred = N_pred; // If we do another loop, start at the begin
461  // of new predictions
462 
463  } while (!missing_predictions_to_add.empty());
464 
465  const double tim_obs_DA = m_timLogger.leave("KF:7.get obs & DA");
466 
467  // =============================================================
468  // 7. UPDATE USING THE KALMAN GAIN
469  // =============================================================
470  // Update, only if there are observations!
471  if (!Z.empty())
472  {
473  m_timLogger.enter("KF:8.update stage");
474 
475  switch (KF_options.method)
476  {
477  // -----------------------
478  // FULL KF- METHOD
479  // -----------------------
480  case kfEKFNaive:
481  case kfIKFFull:
482  {
483  // Build the whole Jacobian dh_dx matrix
484  // ---------------------------------------------
485  // Keep only those whose DA is not -1
486  std::vector<int> mapIndicesForKFUpdate(data_association.size());
487  mapIndicesForKFUpdate.resize(std::distance(
488  mapIndicesForKFUpdate.begin(),
489  std::remove_copy_if(
490  data_association.begin(), data_association.end(),
491  mapIndicesForKFUpdate.begin(),
492  [](int i) { return i == -1; })));
493 
494  const size_t N_upd =
495  (FEAT_SIZE == 0) ? 1 : // Non-SLAM problems: Just one
496  // observation for the entire
497  // system.
498  mapIndicesForKFUpdate
499  .size(); // SLAM: # of observed known landmarks
500 
501  // Just one, or several update iterations??
502  const size_t nKF_iterations = (KF_options.method == kfEKFNaive)
503  ? 1
504  : KF_options.IKF_iterations;
505 
506  const KFVector xkk_0 = m_xkk;
507 
508  // For each IKF iteration (or 1 for EKF)
509  if (N_upd > 0) // Do not update if we have no observations!
510  {
511  for (size_t IKF_iteration = 0;
512  IKF_iteration < nKF_iterations; IKF_iteration++)
513  {
514  // Compute ytilde = OBS - PREDICTION
515  KFVector ytilde(OBS_SIZE * N_upd);
516  size_t ytilde_idx = 0;
517 
518  // TODO: Use a Matrix view of "dh_dx_full" instead of
519  // creating a copy into "dh_dx_full_obs"
520  dh_dx_full_obs.zeros(
521  N_upd * OBS_SIZE,
522  VEH_SIZE + FEAT_SIZE * N_map); // Init to zeros.
523  KFMatrix S_observed; // The KF "S" matrix: A
524  // re-ordered, subset, version of
525  // the prediction S:
526 
527  if (FEAT_SIZE != 0)
528  { // SLAM problems:
529  std::vector<size_t> S_idxs;
530  S_idxs.reserve(OBS_SIZE * N_upd);
531 
532  // const size_t row_len = VEH_SIZE + FEAT_SIZE *
533  // N_map;
534 
535  for (size_t i = 0; i < data_association.size(); ++i)
536  {
537  if (data_association[i] < 0) continue;
538 
539  const size_t assoc_idx_in_map =
540  static_cast<size_t>(data_association[i]);
541  const size_t assoc_idx_in_pred =
543  assoc_idx_in_map, predictLMidxs);
544  ASSERTMSG_(
545  assoc_idx_in_pred != string::npos,
546  "OnPreComputingPredictions() didn't "
547  "recommend the prediction of a landmark "
548  "which has been actually observed!");
549  // TODO: In these cases, extend the prediction
550  // right now instead of launching an
551  // exception... or is this a bad idea??
552 
553  // Build the subset dh_dx_full_obs:
554  // dh_dx_full_obs.block(S_idxs.size()
555  //,0, OBS_SIZE, row_len)
556  // =
557  // dh_dx_full.block
558  //(assoc_idx_in_pred*OBS_SIZE,0, OBS_SIZE,
559  // row_len);
560 
561  Eigen::Block<
562  typename KFMatrix::Base, OBS_SIZE,
563  VEH_SIZE>(
564  dh_dx_full_obs, S_idxs.size(), 0) =
565  Hxs[assoc_idx_in_pred];
566  Eigen::Block<
567  typename KFMatrix::Base, OBS_SIZE,
568  FEAT_SIZE>(
569  dh_dx_full_obs, S_idxs.size(),
570  VEH_SIZE + assoc_idx_in_map * FEAT_SIZE) =
571  Hys[assoc_idx_in_pred];
572 
573  // S_idxs.size() is used as counter for
574  // "dh_dx_full_obs".
575  for (size_t k = 0; k < OBS_SIZE; k++)
576  S_idxs.push_back(
577  assoc_idx_in_pred * OBS_SIZE + k);
578 
579  // ytilde_i = Z[i] - all_predictions[i]
580  KFArray_OBS ytilde_i = Z[i];
581  OnSubstractObservationVectors(
582  ytilde_i,
583  all_predictions
584  [predictLMidxs[assoc_idx_in_pred]]);
585  for (size_t k = 0; k < OBS_SIZE; k++)
586  ytilde[ytilde_idx++] = ytilde_i[k];
587  }
588  // Extract the subset that is involved in this
589  // observation:
590  S.extractSubmatrixSymmetrical(S_idxs, S_observed);
591  }
592  else
593  { // Non-SLAM problems:
594  ASSERT_(
595  Z.size() == 1 && all_predictions.size() == 1);
596  ASSERT_(
597  dh_dx_full_obs.rows() == OBS_SIZE &&
598  dh_dx_full_obs.cols() == VEH_SIZE);
599  ASSERT_(Hxs.size() == 1);
600  dh_dx_full_obs = Hxs[0]; // Was: dh_dx_full
601  KFArray_OBS ytilde_i = Z[0];
602  OnSubstractObservationVectors(
603  ytilde_i, all_predictions[0]);
604  for (size_t k = 0; k < OBS_SIZE; k++)
605  ytilde[ytilde_idx++] = ytilde_i[k];
606  // Extract the subset that is involved in this
607  // observation:
608  S_observed = S;
609  }
610 
611  // Compute the full K matrix:
612  // ------------------------------
613  m_timLogger.enter("KF:8.update stage:1.FULLKF:build K");
614 
615  K.setSize(m_pkk.rows(), S_observed.cols());
616 
617  // K = m_pkk * (~dh_dx) * S.inv() );
618  K.multiply_ABt(m_pkk, dh_dx_full_obs);
619 
620  // Inverse of S_observed -> S_1
621  S_observed.inv(S_1);
622  K *= S_1;
623 
624  m_timLogger.leave("KF:8.update stage:1.FULLKF:build K");
625 
626  // Use the full K matrix to update the mean:
627  if (nKF_iterations == 1)
628  {
629  m_timLogger.enter(
630  "KF:8.update stage:2.FULLKF:update xkk");
631  m_xkk += K * ytilde;
632  m_timLogger.leave(
633  "KF:8.update stage:2.FULLKF:update xkk");
634  }
635  else
636  {
637  m_timLogger.enter(
638  "KF:8.update stage:2.FULLKF:iter.update xkk");
639 
640  KFVector HAx_column;
641  dh_dx_full_obs.multiply_Ab(
642  m_xkk - xkk_0, HAx_column);
643 
644  m_xkk = xkk_0;
645  K.multiply_Ab(
646  (ytilde - HAx_column), m_xkk,
647  true /* Accumulate in output */
648  );
649 
650  m_timLogger.leave(
651  "KF:8.update stage:2.FULLKF:iter.update xkk");
652  }
653 
654  // Update the covariance just at the end
655  // of iterations if we are in IKF, always in normal
656  // EKF.
657  if (IKF_iteration == (nKF_iterations - 1))
658  {
659  m_timLogger.enter(
660  "KF:8.update stage:3.FULLKF:update Pkk");
661 
662  // Use the full K matrix to update the covariance:
663  // m_pkk = (I - K*dh_dx ) * m_pkk;
664  // TODO: "Optimize this: sparsity!"
665 
666  // K * dh_dx_full_obs
667  aux_K_dh_dx.multiply(K, dh_dx_full_obs);
668 
669  // aux_K_dh_dx <-- I-aux_K_dh_dx
670  const size_t stat_len = aux_K_dh_dx.cols();
671  for (size_t r = 0; r < stat_len; r++)
672  {
673  for (size_t c = 0; c < stat_len; c++)
674  {
675  if (r == c)
676  aux_K_dh_dx.get_unsafe(r, c) =
677  -aux_K_dh_dx.get_unsafe(r, c) +
678  kftype(1);
679  else
680  aux_K_dh_dx.get_unsafe(r, c) =
681  -aux_K_dh_dx.get_unsafe(r, c);
682  }
683  }
684 
685  m_pkk.multiply_result_is_symmetric(
686  aux_K_dh_dx, m_pkk);
687 
688  m_timLogger.leave(
689  "KF:8.update stage:3.FULLKF:update Pkk");
690  }
691  } // end for each IKF iteration
692  }
693  }
694  break;
695 
696  // --------------------------------------------------------------------
697  // - EKF 'a la' Davison: One observation element at once
698  // --------------------------------------------------------------------
699  case kfEKFAlaDavison:
700  {
701  // For each observed landmark/whole system state:
702  for (size_t obsIdx = 0; obsIdx < Z.size(); obsIdx++)
703  {
704  // Known & mapped landmark?
705  bool doit;
706  size_t idxInTheFilter = 0;
707 
708  if (data_association.empty())
709  {
710  doit = true;
711  }
712  else
713  {
714  doit = data_association[obsIdx] >= 0;
715  if (doit) idxInTheFilter = data_association[obsIdx];
716  }
717 
718  if (doit)
719  {
720  m_timLogger.enter(
721  "KF:8.update stage:1.ScalarAtOnce.prepare");
722 
723  // Already mapped: OK
724  const size_t idx_off =
725  VEH_SIZE +
726  idxInTheFilter *
727  FEAT_SIZE; // The offset in m_xkk & Pkk.
728 
729  // Compute just the part of the Jacobian that we need
730  // using the current updated m_xkk:
731  vector_KFArray_OBS pred_obs;
732  OnObservationModel(
733  std::vector<size_t>(1, idxInTheFilter), pred_obs);
734  ASSERTDEB_(pred_obs.size() == 1);
735 
736  // ytilde = observation - prediction
737  KFArray_OBS ytilde = Z[obsIdx];
738  OnSubstractObservationVectors(ytilde, pred_obs[0]);
739 
740  // Jacobians:
741  // dh_dx: already is (N_pred*OBS_SIZE) x (VEH_SIZE +
742  // FEAT_SIZE * N_pred )
743  // with N_pred = |predictLMidxs|
744 
745  const size_t i_idx_in_preds =
747  idxInTheFilter, predictLMidxs);
748  ASSERTMSG_(
749  i_idx_in_preds != string::npos,
750  "OnPreComputingPredictions() didn't recommend the "
751  "prediction of a landmark which has been actually "
752  "observed!");
753 
754  const KFMatrix_OxV& Hx = Hxs[i_idx_in_preds];
755  const KFMatrix_OxF& Hy = Hys[i_idx_in_preds];
756 
757  m_timLogger.leave(
758  "KF:8.update stage:1.ScalarAtOnce.prepare");
759 
760  // For each component of the observation:
761  for (size_t j = 0; j < OBS_SIZE; j++)
762  {
763  m_timLogger.enter(
764  "KF:8.update stage:2.ScalarAtOnce.update");
765 
766  // Compute the scalar S_i for each component j of
767  // the observation: Sij = dhij_dxv Pxx dhij_dxv^t +
768  // 2 * dhij_dyi Pyix dhij_dxv + dhij_dyi Pyiyi
769  // dhij_dyi^t + R
770  // ^^
771  // Hx:(O*LxSv)
772  // \----------------------/
773  // \--------------------------/
774  // \------------------------/ \-/
775  // TERM 1 TERM 2
776  // TERM 3 R
777  //
778  // O: Observation size (3)
779  // L: # landmarks
780  // Sv: Vehicle state size (6)
781  //
782 
783 #if defined(_DEBUG)
784  {
785  // This algorithm is applicable only if the
786  // scalar component of the sensor noise are
787  // INDEPENDENT:
788  for (size_t a = 0; a < OBS_SIZE; a++)
789  for (size_t b = 0; b < OBS_SIZE; b++)
790  if (a != b)
791  if (R(a, b) != 0)
793  "This KF algorithm assumes "
794  "independent noise "
795  "components in the "
796  "observation (matrix R). "
797  "Select another KF "
798  "algorithm.")
799  }
800 #endif
801  // R:
802  KFTYPE Sij = R.get_unsafe(j, j);
803 
804  // TERM 1:
805  for (size_t k = 0; k < VEH_SIZE; k++)
806  {
807  KFTYPE accum = 0;
808  for (size_t q = 0; q < VEH_SIZE; q++)
809  accum += Hx.get_unsafe(j, q) *
810  m_pkk.get_unsafe(q, k);
811  Sij += Hx.get_unsafe(j, k) * accum;
812  }
813 
814  // TERM 2:
815  KFTYPE term2 = 0;
816  for (size_t k = 0; k < VEH_SIZE; k++)
817  {
818  KFTYPE accum = 0;
819  for (size_t q = 0; q < FEAT_SIZE; q++)
820  accum += Hy.get_unsafe(j, q) *
821  m_pkk.get_unsafe(idx_off + q, k);
822  term2 += Hx.get_unsafe(j, k) * accum;
823  }
824  Sij += 2 * term2;
825 
826  // TERM 3:
827  for (size_t k = 0; k < FEAT_SIZE; k++)
828  {
829  KFTYPE accum = 0;
830  for (size_t q = 0; q < FEAT_SIZE; q++)
831  accum += Hy.get_unsafe(j, q) *
832  m_pkk.get_unsafe(
833  idx_off + q, idx_off + k);
834  Sij += Hy.get_unsafe(j, k) * accum;
835  }
836 
837  // Compute the Kalman gain "Kij" for this
838  // observation element:
839  // --> K = m_pkk * (~dh_dx) * S.inv() );
840  size_t N = m_pkk.cols();
841  vector<KFTYPE> Kij(N);
842 
843  for (size_t k = 0; k < N; k++)
844  {
845  KFTYPE K_tmp = 0;
846 
847  // dhi_dxv term
848  size_t q;
849  for (q = 0; q < VEH_SIZE; q++)
850  K_tmp += m_pkk.get_unsafe(k, q) *
851  Hx.get_unsafe(j, q);
852 
853  // dhi_dyi term
854  for (q = 0; q < FEAT_SIZE; q++)
855  K_tmp += m_pkk.get_unsafe(k, idx_off + q) *
856  Hy.get_unsafe(j, q);
857 
858  Kij[k] = K_tmp / Sij;
859  } // end for k
860 
861  // Update the state vector m_xkk:
862  // x' = x + Kij * ytilde(ij)
863  for (size_t k = 0; k < N; k++)
864  m_xkk[k] += Kij[k] * ytilde[j];
865 
866  // Update the covariance Pkk:
867  // P' = P - Kij * Sij * Kij^t
868  {
869  for (size_t k = 0; k < N; k++)
870  {
871  for (size_t q = k; q < N;
872  q++) // Half matrix
873  {
874  m_pkk(k, q) -= Sij * Kij[k] * Kij[q];
875  // It is symmetric
876  m_pkk(q, k) = m_pkk(k, q);
877  }
878 
879 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
880  if (m_pkk(k, k) < 0)
881  {
882  m_pkk.saveToTextFile("Pkk_err.txt");
884  Kij, "Kij.txt");
885  ASSERT_(m_pkk(k, k) > 0);
886  }
887 #endif
888  }
889  }
890 
891  m_timLogger.leave(
892  "KF:8.update stage:2.ScalarAtOnce.update");
893  } // end for j
894  } // end if mapped
895  } // end for each observed LM.
896  }
897  break;
898 
899  // --------------------------------------------------------------------
900  // - IKF method, processing each observation scalar secuentially:
901  // --------------------------------------------------------------------
902  case kfIKF: // TODO !!
903  {
904  THROW_EXCEPTION("IKF scalar by scalar not implemented yet.");
905 #if 0
906  KFMatrix h,Hx,Hy;
907 
908  // Just one, or several update iterations??
909  size_t nKF_iterations = KF_options.IKF_iterations;
910 
911  // To avoid wasting time, if we are doing 1 iteration only, do not reserve memory for this matrix:
912  KFMatrix *saved_Pkk=nullptr;
913  if (nKF_iterations>1)
914  {
915  // Create a copy of Pkk for later restoring it at the beginning of each iteration:
916  saved_Pkk = new KFMatrix( m_pkk );
917  }
918 
919  KFVector xkk_0 = m_xkk; // First state vector at the beginning of IKF
920  KFVector xkk_next_iter = m_xkk; // for IKF only
921 
922  // For each IKF iteration (or 1 for EKF)
923  for (size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
924  {
925  // Restore initial covariance matrix.
926  // The updated mean is stored in m_xkk and is improved with each estimation,
927  // and so do the Jacobians which use that improving mean.
928  if (IKF_iteration>0)
929  {
930  m_pkk = *saved_Pkk;
931  xkk_next_iter = xkk_0;
932  }
933 
934  // For each observed landmark/whole system state:
935  for (size_t obsIdx=0;obsIdx<Z.rows();obsIdx++)
936  {
937  // Known & mapped landmark?
938  bool doit;
939  size_t idxInTheFilter=0;
940 
941  if (data_association.empty())
942  {
943  doit = true;
944  }
945  else
946  {
947  doit = data_association[obsIdx] >= 0;
948  if (doit)
949  idxInTheFilter = data_association[obsIdx];
950  }
951 
952  if ( doit )
953  {
954  // Already mapped: OK
955  const size_t idx_off = VEH_SIZE + idxInTheFilter *FEAT_SIZE; // The offset in m_xkk & Pkk.
956  const size_t R_row_offset = obsIdx*OBS_SIZE; // Offset row in R
957 
958  // Compute just the part of the Jacobian that we need using the current updated m_xkk:
959  KFVector ytilde; // Diff. observation - prediction
960  OnObservationModelAndJacobians(
961  Z,
962  data_association,
963  false, // Only a partial Jacobian
964  (int)obsIdx, // Which row from Z
965  ytilde,
966  Hx,
967  Hy );
968 
969  ASSERTDEB_(ytilde.size() == OBS_SIZE )
970  ASSERTDEB_(Hx.rows() == OBS_SIZE )
971  ASSERTDEB_(Hx.cols() == VEH_SIZE )
972 
973  if (FEAT_SIZE>0)
974  {
975  ASSERTDEB_(Hy.rows() == OBS_SIZE )
976  ASSERTDEB_(Hy.cols() == FEAT_SIZE )
977  }
978 
979  // Compute the OxO matrix S_i for each observation:
980  // Si = TERM1 + TERM2 + TERM2^t + TERM3 + R
981  //
982  // TERM1: dhi_dxv Pxx dhi_dxv^t
983  // ^^
984  // Hx:(OxV)
985  //
986  // TERM2: dhi_dyi Pyix dhi_dxv
987  // ^^
988  // Hy:(OxF)
989  //
990  // TERM3: dhi_dyi Pyiyi dhi_dyi^t
991  //
992  // i: obsIdx
993  // O: Observation size
994  // F: Feature size
995  // V: Vehicle state size
996  //
997 
998  // R:
999  KFMatrix Si(OBS_SIZE,OBS_SIZE);
1000  R.extractMatrix(R_row_offset,0, Si);
1001 
1002  size_t k;
1003  KFMatrix term(OBS_SIZE,OBS_SIZE);
1004 
1005  // TERM1: dhi_dxv Pxx dhi_dxv^t
1006  Hx.multiply_HCHt(
1007  m_pkk, // The cov. matrix
1008  Si, // The output
1009  true, // Yes, submatrix of m_pkk only
1010  0, // Offset in m_pkk
1011  true // Yes, accum results in output.
1012  );
1013 
1014  // TERM2: dhi_dyi Pyix dhi_dxv
1015  // + its transpose:
1016  KFMatrix Pyix( FEAT_SIZE, VEH_SIZE );
1017  m_pkk.extractMatrix(idx_off,0, Pyix); // Extract cross cov. to Pyix
1018 
1019  term.multiply_ABCt( Hy, Pyix, Hx ); //term = Hy * Pyix * ~Hx;
1020  Si.add_AAt( term ); // Si += A + ~A
1021 
1022  // TERM3: dhi_dyi Pyiyi dhi_dyi^t
1023  Hy.multiply_HCHt(
1024  m_pkk, // The cov. matrix
1025  Si, // The output
1026  true, // Yes, submatrix of m_pkk only
1027  idx_off, // Offset in m_pkk
1028  true // Yes, accum results in output.
1029  );
1030 
1031  // Compute the inverse of Si:
1032  KFMatrix Si_1(OBS_SIZE,OBS_SIZE);
1033 
1034  // Compute the Kalman gain "Ki" for this i'th observation:
1035  // --> Ki = m_pkk * (~dh_dx) * S.inv();
1036  size_t N = m_pkk.cols();
1037 
1038  KFMatrix Ki( N, OBS_SIZE );
1039 
1040  for (k=0;k<N;k++) // for each row of K
1041  {
1042  size_t q;
1043 
1044  for (size_t c=0;c<OBS_SIZE;c++) // for each column of K
1045  {
1046  KFTYPE K_tmp = 0;
1047 
1048  // dhi_dxv term
1049  for (q=0;q<VEH_SIZE;q++)
1050  K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(c,q);
1051 
1052  // dhi_dyi term
1053  for (q=0;q<FEAT_SIZE;q++)
1054  K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(c,q);
1055 
1056  Ki.set_unsafe(k,c, K_tmp);
1057  } // end for c
1058  } // end for k
1059 
1060  Ki.multiply(Ki, Si.inv() ); // K = (...) * S^-1
1061 
1062 
1063  // Update the state vector m_xkk:
1064  if (nKF_iterations==1)
1065  {
1066  // EKF:
1067  // x' = x + Kij * ytilde(ij)
1068  for (k=0;k<N;k++)
1069  for (size_t q=0;q<OBS_SIZE;q++)
1070  m_xkk[k] += Ki.get_unsafe(k,q) * ytilde[q];
1071  }
1072  else
1073  {
1074  // IKF:
1075  Eigen::Matrix<KFTYPE,Eigen::Dynamic,1> HAx(OBS_SIZE);
1076  size_t o,q;
1077  // HAx = H*(x0-xi)
1078  for (o=0;o<OBS_SIZE;o++)
1079  {
1080  KFTYPE tmp = 0;
1081  for (q=0;q<VEH_SIZE;q++)
1082  tmp += Hx.get_unsafe(o,q) * (xkk_0[q] - m_xkk[q]);
1083 
1084  for (q=0;q<FEAT_SIZE;q++)
1085  tmp += Hy.get_unsafe(o,q) * (xkk_0[idx_off+q] - m_xkk[idx_off+q]);
1086 
1087  HAx[o] = tmp;
1088  }
1089 
1090  // Compute only once (ytilde[j] - HAx)
1091  for (o=0;o<OBS_SIZE;o++)
1092  HAx[o] = ytilde[o] - HAx[o];
1093 
1094  // x' = x_0 + Kij * ( ytilde(ij) - H*(x0-xi)) -> xi: i=this iteration
1095  // xkk_next_iter is initialized to xkk_0 at each IKF iteration.
1096  for (k=0;k<N;k++)
1097  {
1098  KFTYPE tmp = xkk_next_iter[k];
1099  //m_xkk[k] = xkk_0[k] + Kij[k] * (ytilde[j] - HAx );
1100  for (o=0;o<OBS_SIZE;o++)
1101  tmp += Ki.get_unsafe(k,o) * HAx[o];
1102 
1103  xkk_next_iter[k] = tmp;
1104  }
1105  }
1106 
1107  // Update the covariance Pkk:
1108  // P' = P - Kij * Sij * Kij^t
1109  //if (IKF_iteration==(nKF_iterations-1)) // Just for the last IKF iteration
1110  {
1111  // m_pkk -= Ki * Si * ~Ki;
1112  Ki.multiplyByMatrixAndByTransposeNonSymmetric(
1113  Si,
1114  m_pkk, // Output
1115  true, // Accumulate
1116  true); // Substract instead of sum.
1117 
1118  m_pkk.force_symmetry();
1119 
1120  /* for (k=0;k<N;k++)
1121  {
1122  for (size_t q=k;q<N;q++) // Half matrix
1123  {
1124  m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
1125  // It is symmetric
1126  m_pkk(q,k) = m_pkk(k,q);
1127  }
1128 
1129  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
1130  if (m_pkk(k,k)<0)
1131  {
1132  m_pkk.saveToTextFile("Pkk_err.txt");
1133  mrpt::system::vectorToTextFile(Kij,"Kij.txt");
1134  ASSERT_(m_pkk(k,k)>0)
1135  }
1136  #endif
1137  }
1138  */
1139  }
1140 
1141  } // end if doit
1142 
1143  } // end for each observed LM.
1144 
1145  // Now, save the new iterated mean:
1146  if (nKF_iterations>1)
1147  {
1148 #if 0
1149  cout << "IKF iter: " << IKF_iteration << " -> " << xkk_next_iter << endl;
1150 #endif
1151  m_xkk = xkk_next_iter;
1152  }
1153 
1154  } // end for each IKF_iteration
1155 
1156  // Copy of pkk not required anymore:
1157  if (saved_Pkk) delete saved_Pkk;
1158 
1159 #endif
1160  }
1161  break;
1162 
1163  default:
1164  THROW_EXCEPTION("Invalid value of options.KF_method");
1165  } // end switch method
1166  }
1167 
1168  const double tim_update = m_timLogger.leave("KF:8.update stage");
1169 
1170  m_timLogger.enter("KF:9.OnNormalizeStateVector");
1171  OnNormalizeStateVector();
1172  m_timLogger.leave("KF:9.OnNormalizeStateVector");
1173 
1174  // =============================================================
1175  // 8. INTRODUCE NEW LANDMARKS
1176  // =============================================================
1177  if (!data_association.empty())
1178  {
1179  m_timLogger.enter("KF:A.add new landmarks");
1180  detail::addNewLandmarks(*this, Z, data_association, R);
1181  m_timLogger.leave("KF:A.add new landmarks");
1182  } // end if data_association!=empty
1183 
1184  // Post iteration user code:
1185  m_timLogger.enter("KF:B.OnPostIteration");
1186  OnPostIteration();
1187  m_timLogger.leave("KF:B.OnPostIteration");
1188 
1189  m_timLogger.leave("KF:complete_step");
1190 
1192  "[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: "
1193  "%.2fms\n",
1194  static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
1195  1e3 * tim_pred, 1e3 * tim_pred_obs, 1e3 * tim_obs_DA,
1196  1e3 * tim_update));
1197  MRPT_END
1198 } // End of "runOneKalmanIteration"
1199 
1200 template <
1201  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1202  typename KFTYPE>
1205  const KFArray_VEH& x, const std::pair<KFCLASS*, KFArray_ACT>& dat,
1206  KFArray_VEH& out_x)
1207 {
1208  bool dumm;
1209  out_x = x;
1210  dat.first->OnTransitionModel(dat.second, out_x, dumm);
1211 }
1212 
1213 template <
1214  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1215  typename KFTYPE>
1218  const KFArray_VEH& x, const std::pair<KFCLASS*, size_t>& dat,
1219  KFArray_OBS& out_x)
1220 {
1221  std::vector<size_t> idxs_to_predict(1, dat.second);
1222  vector_KFArray_OBS prediction;
1223  // Overwrite (temporarily!) the affected part of the state vector:
1224  ::memcpy(&dat.first->m_xkk[0], &x[0], sizeof(x[0]) * VEH_SIZE);
1225  dat.first->OnObservationModel(idxs_to_predict, prediction);
1226  ASSERTDEB_(prediction.size() == 1);
1227  out_x = prediction[0];
1228 }
1229 
1230 template <
1231  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1232  typename KFTYPE>
1235  const KFArray_FEAT& x, const std::pair<KFCLASS*, size_t>& dat,
1236  KFArray_OBS& out_x)
1237 {
1238  std::vector<size_t> idxs_to_predict(1, dat.second);
1239  vector_KFArray_OBS prediction;
1240  const size_t lm_idx_in_statevector = VEH_SIZE + FEAT_SIZE * dat.second;
1241  // Overwrite (temporarily!) the affected part of the state vector:
1242  ::memcpy(
1243  &dat.first->m_xkk[lm_idx_in_statevector], &x[0],
1244  sizeof(x[0]) * FEAT_SIZE);
1245  dat.first->OnObservationModel(idxs_to_predict, prediction);
1246  ASSERTDEB_(prediction.size() == 1);
1247  out_x = prediction[0];
1248 }
1249 
1250 namespace detail
1251 {
1252 // generic version for SLAM. There is a speciation below for NON-SLAM problems.
1253 template <
1254  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1255  typename KFTYPE>
1258  const typename CKalmanFilterCapable<
1259  VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
1260  const std::vector<int>& data_association,
1261  const typename CKalmanFilterCapable<
1262  VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::KFMatrix_OxO& R)
1263 {
1264  using KF =
1266 
1267  for (size_t idxObs = 0; idxObs < Z.size(); idxObs++)
1268  {
1269  // Is already in the map?
1270  if (data_association[idxObs] < 0) // Not in the map yet!
1271  {
1272  obj.getProfiler().enter("KF:9.create new LMs");
1273  // Add it:
1274 
1275  // Append to map of IDs <-> position in the state vector:
1276  ASSERTDEB_(FEAT_SIZE > 0);
1277  ASSERTDEB_(
1278  0 == ((obj.internal_getXkk().size() - VEH_SIZE) %
1279  FEAT_SIZE)); // Sanity test
1280 
1281  const size_t newIndexInMap =
1282  (obj.internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
1283 
1284  // Inverse sensor model:
1285  typename KF::KFArray_FEAT yn;
1286  typename KF::KFMatrix_FxV dyn_dxv;
1287  typename KF::KFMatrix_FxO dyn_dhn;
1288  typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1289  bool use_dyn_dhn_jacobian = true;
1290 
1291  // Compute the inv. sensor model and its Jacobians:
1292  obj.OnInverseObservationModel(
1293  Z[idxObs], yn, dyn_dxv, dyn_dhn, dyn_dhn_R_dyn_dhnT,
1294  use_dyn_dhn_jacobian);
1295 
1296  // And let the application do any special handling of adding a new
1297  // feature to the map:
1298  obj.OnNewLandmarkAddedToMap(idxObs, newIndexInMap);
1299 
1300  ASSERTDEB_(yn.size() == FEAT_SIZE);
1301 
1302  // Append to m_xkk:
1303  size_t q;
1304  size_t idx = obj.internal_getXkk().size();
1305  obj.internal_getXkk().conservativeResize(
1306  obj.internal_getXkk().size() + FEAT_SIZE);
1307 
1308  for (q = 0; q < FEAT_SIZE; q++)
1309  obj.internal_getXkk()[idx + q] = yn[q];
1310 
1311  // --------------------
1312  // Append to Pkk:
1313  // --------------------
1314  ASSERTDEB_(
1315  obj.internal_getPkk().cols() == (int)idx &&
1316  obj.internal_getPkk().rows() == (int)idx);
1317 
1318  obj.internal_getPkk().setSize(idx + FEAT_SIZE, idx + FEAT_SIZE);
1319 
1320  // Fill the Pxyn term:
1321  // --------------------
1322  typename KF::KFMatrix_VxV Pxx;
1323  obj.internal_getPkk().extractMatrix(0, 0, Pxx);
1324  typename KF::KFMatrix_FxV Pxyn; // Pxyn = dyn_dxv * Pxx
1325  Pxyn.multiply(dyn_dxv, Pxx);
1326 
1327  obj.internal_getPkk().insertMatrix(idx, 0, Pxyn);
1328  obj.internal_getPkk().insertMatrixTranspose(0, idx, Pxyn);
1329 
1330  // Fill the Pyiyn terms:
1331  // --------------------
1332  const size_t nLMs =
1333  (idx - VEH_SIZE) / FEAT_SIZE; // Number of previous landmarks:
1334  for (q = 0; q < nLMs; q++)
1335  {
1336  typename KF::KFMatrix_VxF P_x_yq(
1338  obj.internal_getPkk().extractMatrix(
1339  0, VEH_SIZE + q * FEAT_SIZE, P_x_yq);
1340 
1341  typename KF::KFMatrix_FxF P_cross(
1343  P_cross.multiply(dyn_dxv, P_x_yq);
1344 
1345  obj.internal_getPkk().insertMatrix(
1346  idx, VEH_SIZE + q * FEAT_SIZE, P_cross);
1347  obj.internal_getPkk().insertMatrixTranspose(
1348  VEH_SIZE + q * FEAT_SIZE, idx, P_cross);
1349  } // end each previous LM(q)
1350 
1351  // Fill the Pynyn term:
1352  // P_yn_yn = (dyn_dxv * Pxx * ~dyn_dxv) + (dyn_dhn * R *
1353  // ~dyn_dhn);
1354  // --------------------
1355  typename KF::KFMatrix_FxF P_yn_yn(mrpt::math::UNINITIALIZED_MATRIX);
1356  dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
1357  if (use_dyn_dhn_jacobian)
1358  dyn_dhn.multiply_HCHt(
1359  R, P_yn_yn, true); // Accumulate in P_yn_yn
1360  else
1361  P_yn_yn += dyn_dhn_R_dyn_dhnT;
1362 
1363  obj.internal_getPkk().insertMatrix(idx, idx, P_yn_yn);
1364 
1365  obj.getProfiler().leave("KF:9.create new LMs");
1366  }
1367  }
1368 }
1369 
1370 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1373  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE, KFTYPE>& obj,
1374  const typename CKalmanFilterCapable<
1375  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
1376  KFTYPE>::vector_KFArray_OBS& Z,
1377  const std::vector<int>& data_association,
1378  const typename CKalmanFilterCapable<
1379  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
1380  KFTYPE>::KFMatrix_OxO& R)
1381 {
1382  // Do nothing: this is NOT a SLAM problem.
1383 }
1384 
1385 template <
1386  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1387  typename KFTYPE>
1390  obj)
1391 {
1392  return (obj.getStateVectorLength() - VEH_SIZE) / FEAT_SIZE;
1393 }
1394 // Specialization for FEAT_SIZE=0
1395 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1397  const CKalmanFilterCapable<
1398  VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj)
1399 {
1400  return 0;
1401 }
1402 
1403 template <
1404  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1405  typename KFTYPE>
1406 inline bool isMapEmpty(
1408  obj)
1409 {
1410  return (obj.getStateVectorLength() == VEH_SIZE);
1411 }
1412 // Specialization for FEAT_SIZE=0
1413 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1414 inline bool isMapEmpty(
1415  const CKalmanFilterCapable<
1416  VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj)
1417 {
1418  return true;
1419 }
1420 } // namespace detail
1421 } // namespace bayes
1422 } // namespace mrpt
1423 
1424 #endif
Eigen::Matrix< double, Eigen::Dynamic, 1 > KFVector
#define MRPT_START
Definition: exceptions.h:262
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
STL namespace.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
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 estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, 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...
Definition: num_jacobian.h:31
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
A numeric matrix of compile-time fixed size.
const GLubyte * c
Definition: glext.h:6313
void addNewLandmarks(CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::vector_KFArray_OBS &Z, const std::vector< int > &data_association, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO &R)
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
static void KF_aux_estimate_trans_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, KFArray_ACT > &dat, KFArray_VEH &out_x)
Auxiliary functions for Jacobian numeric estimation.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
double kftype
The numeric type used in the Kalman Filter (default=double)
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
const float R
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::AutoAlign|Eigen::RowMajor > Base
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
#define MRPT_END
Definition: exceptions.h:266
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
GLsizeiptr size
Definition: glext.h:3923
GLenum GLint x
Definition: glext.h:3538
bool vectorToTextFile(const std::vector< float > &vec, const std::string &fileName, bool append=false, bool byRows=false)
A useful function for debugging, which saves a std::vector into a text file (compat.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
mrpt::system::CTimeLogger & getProfiler()
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
void enter(const char *func_name)
Start of a named section.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:356



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019