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



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019