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


Page generated by Doxygen 1.8.8 for MRPT 1.2.2 SVN:Unversioned directory at Tue Oct 14 02:14:08 UTC 2014