Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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 || KF_options.verbose);
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; // Wether 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  printf_debug("[KF] *Performance Warning*: %u LMs were not correctly predicted by OnPreComputingPredictions()\n",static_cast<unsigned int>(nNew));
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  binder1st<equal_to<int> >(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  m_pkk.multiply_result_is_symmetric(aux_K_dh_dx, m_pkk );
533 
534  m_timLogger.leave("KF:8.update stage:3.FULLKF:update Pkk");
535  }
536  } // end for each IKF iteration
537  }
538  }
539  break;
540 
541  // --------------------------------------------------------------------
542  // - EKF 'a la' Davison: One observation element at once
543  // --------------------------------------------------------------------
544  case kfEKFAlaDavison:
545  {
546  // For each observed landmark/whole system state:
547  for (size_t obsIdx=0;obsIdx<Z.size();obsIdx++)
548  {
549  // Known & mapped landmark?
550  bool doit;
551  size_t idxInTheFilter=0;
552 
553  if (data_association.empty())
554  {
555  doit = true;
556  }
557  else
558  {
559  doit = data_association[obsIdx] >= 0;
560  if (doit)
561  idxInTheFilter = data_association[obsIdx];
562  }
563 
564  if ( doit )
565  {
566  m_timLogger.enter("KF:8.update stage:1.ScalarAtOnce.prepare");
567 
568  // Already mapped: OK
569  const size_t idx_off = VEH_SIZE + idxInTheFilter*FEAT_SIZE; // The offset in m_xkk & Pkk.
570 
571  // Compute just the part of the Jacobian that we need using the current updated m_xkk:
572  vector_KFArray_OBS pred_obs;
573  OnObservationModel( vector_size_t(1,idxInTheFilter),pred_obs);
574  ASSERTDEB_(pred_obs.size()==1);
575 
576  // ytilde = observation - prediction
577  KFArray_OBS ytilde = Z[obsIdx];
578  OnSubstractObservationVectors(ytilde, pred_obs[0]);
579 
580  // Jacobians:
581  // dh_dx: already is (N_pred*OBS_SIZE) x (VEH_SIZE + FEAT_SIZE * N_pred )
582  // with N_pred = |predictLMidxs|
583 
584  const size_t i_idx_in_preds = mrpt::utils::find_in_vector(idxInTheFilter,predictLMidxs);
585  ASSERTMSG_(i_idx_in_preds!=string::npos, "OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!")
586 
587  const KFMatrix_OxV & Hx = Hxs[i_idx_in_preds];
588  const KFMatrix_OxF & Hy = Hys[i_idx_in_preds];
589 
590  m_timLogger.leave("KF:8.update stage:1.ScalarAtOnce.prepare");
591 
592  // For each component of the observation:
593  for (size_t j=0;j<OBS_SIZE;j++)
594  {
595  m_timLogger.enter("KF:8.update stage:2.ScalarAtOnce.update");
596 
597  // Compute the scalar S_i for each component j of the observation:
598  // Sij = dhij_dxv Pxx dhij_dxv^t + 2 * dhij_dyi Pyix dhij_dxv + dhij_dyi Pyiyi dhij_dyi^t + R
599  // ^^
600  // Hx:(O*LxSv)
601  // \----------------------/ \--------------------------/ \------------------------/ \-/
602  // TERM 1 TERM 2 TERM 3 R
603  //
604  // O: Observation size (3)
605  // L: # landmarks
606  // Sv: Vehicle state size (6)
607  //
608 
609 #if defined(_DEBUG)
610  {
611  // This algorithm is applicable only if the scalar component of the sensor noise are INDEPENDENT:
612  for (size_t a=0;a<OBS_SIZE;a++)
613  for (size_t b=0;b<OBS_SIZE;b++)
614  if ( a!=b )
615  if (R(a,b)!=0)
616  THROW_EXCEPTION("This KF algorithm assumes independent noise components in the observation (matrix R). Select another KF algorithm.")
617  }
618 #endif
619  // R:
620  KFTYPE Sij = R.get_unsafe(j,j);
621 
622  // TERM 1:
623  for (size_t k=0;k<VEH_SIZE;k++)
624  {
625  KFTYPE accum = 0;
626  for (size_t q=0;q<VEH_SIZE;q++)
627  accum += Hx.get_unsafe(j,q) * m_pkk.get_unsafe(q,k);
628  Sij+= Hx.get_unsafe(j,k) * accum;
629  }
630 
631  // TERM 2:
632  KFTYPE term2=0;
633  for (size_t k=0;k<VEH_SIZE;k++)
634  {
635  KFTYPE accum = 0;
636  for (size_t q=0;q<FEAT_SIZE;q++)
637  accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,k);
638  term2+= Hx.get_unsafe(j,k) * accum;
639  }
640  Sij += 2 * term2;
641 
642  // TERM 3:
643  for (size_t k=0;k<FEAT_SIZE;k++)
644  {
645  KFTYPE accum = 0;
646  for (size_t q=0;q<FEAT_SIZE;q++)
647  accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,idx_off+k);
648  Sij+= Hy.get_unsafe(j,k) * accum;
649  }
650 
651  // Compute the Kalman gain "Kij" for this observation element:
652  // --> K = m_pkk * (~dh_dx) * S.inv() );
653  size_t N = m_pkk.getColCount();
654  vector<KFTYPE> Kij( N );
655 
656  for (size_t k=0;k<N;k++)
657  {
658  KFTYPE K_tmp = 0;
659 
660  // dhi_dxv term
661  size_t q;
662  for (q=0;q<VEH_SIZE;q++)
663  K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(j,q);
664 
665  // dhi_dyi term
666  for (q=0;q<FEAT_SIZE;q++)
667  K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(j,q);
668 
669  Kij[k] = K_tmp / Sij;
670  } // end for k
671 
672 
673  // Update the state vector m_xkk:
674  // x' = x + Kij * ytilde(ij)
675  for (size_t k=0;k<N;k++)
676  m_xkk[k] += Kij[k] * ytilde[j];
677 
678  // Update the covariance Pkk:
679  // P' = P - Kij * Sij * Kij^t
680  {
681  for (size_t k=0;k<N;k++)
682  {
683  for (size_t q=k;q<N;q++) // Half matrix
684  {
685  m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
686  // It is symmetric
687  m_pkk(q,k) = m_pkk(k,q);
688  }
689 
690 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
691  if (m_pkk(k,k)<0)
692  {
693  m_pkk.saveToTextFile("Pkk_err.txt");
694  mrpt::system::vectorToTextFile(Kij,"Kij.txt");
695  ASSERT_(m_pkk(k,k)>0)
696  }
697 #endif
698  }
699  }
700 
701 
702  m_timLogger.leave("KF:8.update stage:2.ScalarAtOnce.update");
703  } // end for j
704  } // end if mapped
705  } // end for each observed LM.
706  }
707  break;
708 
709  // --------------------------------------------------------------------
710  // - IKF method, processing each observation scalar secuentially:
711  // --------------------------------------------------------------------
712  case kfIKF: // TODO !!
713  {
714  THROW_EXCEPTION("IKF scalar by scalar not implemented yet.");
715 #if 0
716  KFMatrix h,Hx,Hy;
717 
718  // Just one, or several update iterations??
719  size_t nKF_iterations = KF_options.IKF_iterations;
720 
721  // To avoid wasting time, if we are doing 1 iteration only, do not reserve memory for this matrix:
722  KFMatrix *saved_Pkk=NULL;
723  if (nKF_iterations>1)
724  {
725  // Create a copy of Pkk for later restoring it at the beginning of each iteration:
726  saved_Pkk = new KFMatrix( m_pkk );
727  }
728 
729  KFVector xkk_0 = m_xkk; // First state vector at the beginning of IKF
730  KFVector xkk_next_iter = m_xkk; // for IKF only
731 
732  // For each IKF iteration (or 1 for EKF)
733  for (size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
734  {
735  // Restore initial covariance matrix.
736  // The updated mean is stored in m_xkk and is improved with each estimation,
737  // and so do the Jacobians which use that improving mean.
738  if (IKF_iteration>0)
739  {
740  m_pkk = *saved_Pkk;
741  xkk_next_iter = xkk_0;
742  }
743 
744  // For each observed landmark/whole system state:
745  for (size_t obsIdx=0;obsIdx<Z.getRowCount();obsIdx++)
746  {
747  // Known & mapped landmark?
748  bool doit;
749  size_t idxInTheFilter=0;
750 
751  if (data_association.empty())
752  {
753  doit = true;
754  }
755  else
756  {
757  doit = data_association[obsIdx] >= 0;
758  if (doit)
759  idxInTheFilter = data_association[obsIdx];
760  }
761 
762  if ( doit )
763  {
764  // Already mapped: OK
765  const size_t idx_off = VEH_SIZE + idxInTheFilter *FEAT_SIZE; // The offset in m_xkk & Pkk.
766  const size_t R_row_offset = obsIdx*OBS_SIZE; // Offset row in R
767 
768  // Compute just the part of the Jacobian that we need using the current updated m_xkk:
769  KFVector ytilde; // Diff. observation - prediction
770  OnObservationModelAndJacobians(
771  Z,
772  data_association,
773  false, // Only a partial Jacobian
774  (int)obsIdx, // Which row from Z
775  ytilde,
776  Hx,
777  Hy );
778 
779  ASSERTDEB_(ytilde.size() == OBS_SIZE )
780  ASSERTDEB_(Hx.getRowCount() == OBS_SIZE )
781  ASSERTDEB_(Hx.getColCount() == VEH_SIZE )
782 
783  if (FEAT_SIZE>0)
784  {
785  ASSERTDEB_(Hy.getRowCount() == OBS_SIZE )
786  ASSERTDEB_(Hy.getColCount() == FEAT_SIZE )
787  }
788 
789  // Compute the OxO matrix S_i for each observation:
790  // Si = TERM1 + TERM2 + TERM2^t + TERM3 + R
791  //
792  // TERM1: dhi_dxv Pxx dhi_dxv^t
793  // ^^
794  // Hx:(OxV)
795  //
796  // TERM2: dhi_dyi Pyix dhi_dxv
797  // ^^
798  // Hy:(OxF)
799  //
800  // TERM3: dhi_dyi Pyiyi dhi_dyi^t
801  //
802  // i: obsIdx
803  // O: Observation size
804  // F: Feature size
805  // V: Vehicle state size
806  //
807 
808  // R:
809  KFMatrix Si(OBS_SIZE,OBS_SIZE);
810  R.extractMatrix(R_row_offset,0, Si);
811 
812  size_t k;
813  KFMatrix term(OBS_SIZE,OBS_SIZE);
814 
815  // TERM1: dhi_dxv Pxx dhi_dxv^t
816  Hx.multiply_HCHt(
817  m_pkk, // The cov. matrix
818  Si, // The output
819  true, // Yes, submatrix of m_pkk only
820  0, // Offset in m_pkk
821  true // Yes, accum results in output.
822  );
823 
824  // TERM2: dhi_dyi Pyix dhi_dxv
825  // + its transpose:
826  KFMatrix Pyix( FEAT_SIZE, VEH_SIZE );
827  m_pkk.extractMatrix(idx_off,0, Pyix); // Extract cross cov. to Pyix
828 
829  term.multiply_ABCt( Hy, Pyix, Hx ); //term = Hy * Pyix * ~Hx;
830  Si.add_AAt( term ); // Si += A + ~A
831 
832  // TERM3: dhi_dyi Pyiyi dhi_dyi^t
833  Hy.multiply_HCHt(
834  m_pkk, // The cov. matrix
835  Si, // The output
836  true, // Yes, submatrix of m_pkk only
837  idx_off, // Offset in m_pkk
838  true // Yes, accum results in output.
839  );
840 
841  // Compute the inverse of Si:
842  KFMatrix Si_1(OBS_SIZE,OBS_SIZE);
843 
844  // Compute the Kalman gain "Ki" for this i'th observation:
845  // --> Ki = m_pkk * (~dh_dx) * S.inv();
846  size_t N = m_pkk.getColCount();
847 
848  KFMatrix Ki( N, OBS_SIZE );
849 
850  for (k=0;k<N;k++) // for each row of K
851  {
852  size_t q;
853 
854  for (size_t c=0;c<OBS_SIZE;c++) // for each column of K
855  {
856  KFTYPE K_tmp = 0;
857 
858  // dhi_dxv term
859  for (q=0;q<VEH_SIZE;q++)
860  K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(c,q);
861 
862  // dhi_dyi term
863  for (q=0;q<FEAT_SIZE;q++)
864  K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(c,q);
865 
866  Ki.set_unsafe(k,c, K_tmp);
867  } // end for c
868  } // end for k
869 
870  Ki.multiply(Ki, Si.inv() ); // K = (...) * S^-1
871 
872 
873  // Update the state vector m_xkk:
874  if (nKF_iterations==1)
875  {
876  // EKF:
877  // x' = x + Kij * ytilde(ij)
878  for (k=0;k<N;k++)
879  for (size_t q=0;q<OBS_SIZE;q++)
880  m_xkk[k] += Ki.get_unsafe(k,q) * ytilde[q];
881  }
882  else
883  {
884  // IKF:
885  Eigen::Matrix<KFTYPE,Eigen::Dynamic,1> HAx(OBS_SIZE);
886  size_t o,q;
887  // HAx = H*(x0-xi)
888  for (o=0;o<OBS_SIZE;o++)
889  {
890  KFTYPE tmp = 0;
891  for (q=0;q<VEH_SIZE;q++)
892  tmp += Hx.get_unsafe(o,q) * (xkk_0[q] - m_xkk[q]);
893 
894  for (q=0;q<FEAT_SIZE;q++)
895  tmp += Hy.get_unsafe(o,q) * (xkk_0[idx_off+q] - m_xkk[idx_off+q]);
896 
897  HAx[o] = tmp;
898  }
899 
900  // Compute only once (ytilde[j] - HAx)
901  for (o=0;o<OBS_SIZE;o++)
902  HAx[o] = ytilde[o] - HAx[o];
903 
904  // x' = x_0 + Kij * ( ytilde(ij) - H*(x0-xi)) -> xi: i=this iteration
905  // xkk_next_iter is initialized to xkk_0 at each IKF iteration.
906  for (k=0;k<N;k++)
907  {
908  KFTYPE tmp = xkk_next_iter[k];
909  //m_xkk[k] = xkk_0[k] + Kij[k] * (ytilde[j] - HAx );
910  for (o=0;o<OBS_SIZE;o++)
911  tmp += Ki.get_unsafe(k,o) * HAx[o];
912 
913  xkk_next_iter[k] = tmp;
914  }
915  }
916 
917  // Update the covariance Pkk:
918  // P' = P - Kij * Sij * Kij^t
919  //if (IKF_iteration==(nKF_iterations-1)) // Just for the last IKF iteration
920  {
921  // m_pkk -= Ki * Si * ~Ki;
922  Ki.multiplyByMatrixAndByTransposeNonSymmetric(
923  Si,
924  m_pkk, // Output
925  true, // Accumulate
926  true); // Substract instead of sum.
927 
928  m_pkk.force_symmetry();
929 
930  /* for (k=0;k<N;k++)
931  {
932  for (size_t q=k;q<N;q++) // Half matrix
933  {
934  m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
935  // It is symmetric
936  m_pkk(q,k) = m_pkk(k,q);
937  }
938 
939  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
940  if (m_pkk(k,k)<0)
941  {
942  m_pkk.saveToTextFile("Pkk_err.txt");
943  mrpt::system::vectorToTextFile(Kij,"Kij.txt");
944  ASSERT_(m_pkk(k,k)>0)
945  }
946  #endif
947  }
948  */
949  }
950 
951  } // end if doit
952 
953  } // end for each observed LM.
954 
955  // Now, save the new iterated mean:
956  if (nKF_iterations>1)
957  {
958 #if 0
959  cout << "IKF iter: " << IKF_iteration << " -> " << xkk_next_iter << endl;
960 #endif
961  m_xkk = xkk_next_iter;
962  }
963 
964  } // end for each IKF_iteration
965 
966  // Copy of pkk not required anymore:
967  if (saved_Pkk) delete saved_Pkk;
968 
969 #endif
970  }
971  break;
972 
973  default:
974  THROW_EXCEPTION("Invalid value of options.KF_method");
975  } // end switch method
976 
977  }
978 
979  const double tim_update = m_timLogger.leave("KF:8.update stage");
980 
981  m_timLogger.enter("KF:9.OnNormalizeStateVector");
982  OnNormalizeStateVector();
983  m_timLogger.leave("KF:9.OnNormalizeStateVector");
984 
985  // =============================================================
986  // 8. INTRODUCE NEW LANDMARKS
987  // =============================================================
988  if (!data_association.empty())
989  {
990  m_timLogger.enter("KF:A.add new landmarks");
991  detail::addNewLandmarks(*this, Z, data_association,R);
992  m_timLogger.leave("KF:A.add new landmarks");
993  } // end if data_association!=empty
994 
995  // Post iteration user code:
996  m_timLogger.enter("KF:B.OnPostIteration");
997  OnPostIteration();
998  m_timLogger.leave("KF:B.OnPostIteration");
999 
1000  m_timLogger.leave("KF:complete_step");
1001 
1002  if (KF_options.verbose)
1003  {
1004  printf_debug("[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  }
1012  MRPT_END
1013 
1014  } // End of "runOneKalmanIteration"
1015 
1016 
1017 
1018  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1020  const KFArray_VEH &x,
1021  const std::pair<KFCLASS*,KFArray_ACT> &dat,
1022  KFArray_VEH &out_x)
1023  {
1024  bool dumm;
1025  out_x=x;
1026  dat.first->OnTransitionModel(dat.second,out_x, dumm);
1027  }
1028 
1029  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1031  const KFArray_VEH &x,
1032  const std::pair<KFCLASS*,size_t> &dat,
1033  KFArray_OBS &out_x)
1034  {
1035  vector_size_t idxs_to_predict(1,dat.second);
1036  vector_KFArray_OBS prediction;
1037  // Overwrite (temporarily!) the affected part of the state vector:
1038  ::memcpy(&dat.first->m_xkk[0],&x[0],sizeof(x[0])*VEH_SIZE);
1039  dat.first->OnObservationModel(idxs_to_predict,prediction);
1040  ASSERTDEB_(prediction.size()==1);
1041  out_x=prediction[0];
1042  }
1043 
1044  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1046  const KFArray_FEAT &x,
1047  const std::pair<KFCLASS*,size_t> &dat,
1048  KFArray_OBS &out_x)
1049  {
1050  vector_size_t idxs_to_predict(1,dat.second);
1051  vector_KFArray_OBS prediction;
1052  const size_t lm_idx_in_statevector = VEH_SIZE+FEAT_SIZE*dat.second;
1053  // Overwrite (temporarily!) the affected part of the state vector:
1054  ::memcpy(&dat.first->m_xkk[lm_idx_in_statevector],&x[0],sizeof(x[0])*FEAT_SIZE);
1055  dat.first->OnObservationModel(idxs_to_predict,prediction);
1056  ASSERTDEB_(prediction.size()==1);
1057  out_x=prediction[0];
1058  }
1059 
1060 
1061  namespace detail
1062  {
1063  // generic version for SLAM. There is a speciation below for NON-SLAM problems.
1064  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1068  const vector_int &data_association,
1070  )
1071  {
1073 
1074  for (size_t idxObs=0;idxObs<Z.size();idxObs++)
1075  {
1076  // Is already in the map?
1077  if ( data_association[idxObs] < 0 ) // Not in the map yet!
1078  {
1079  obj.getProfiler().enter("KF:9.create new LMs");
1080  // Add it:
1081 
1082  // Append to map of IDs <-> position in the state vector:
1083  ASSERTDEB_(FEAT_SIZE>0)
1084  ASSERTDEB_( 0 == ((obj.internal_getXkk().size() - VEH_SIZE) % FEAT_SIZE) ) // Sanity test
1085 
1086  const size_t newIndexInMap = (obj.internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
1087 
1088  // Inverse sensor model:
1089  typename KF::KFArray_FEAT yn;
1090  typename KF::KFMatrix_FxV dyn_dxv;
1091  typename KF::KFMatrix_FxO dyn_dhn;
1092  typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1093  bool use_dyn_dhn_jacobian=true;
1094 
1095  // Compute the inv. sensor model and its Jacobians:
1097  Z[idxObs],
1098  yn,
1099  dyn_dxv,
1100  dyn_dhn,
1101  dyn_dhn_R_dyn_dhnT,
1102  use_dyn_dhn_jacobian );
1103 
1104  // And let the application do any special handling of adding a new feature to the map:
1106  idxObs,
1107  newIndexInMap
1108  );
1109 
1110  ASSERTDEB_( yn.size() == FEAT_SIZE )
1111 
1112  // Append to m_xkk:
1113  size_t q;
1114  size_t idx = obj.internal_getXkk().size();
1115  obj.internal_getXkk().conservativeResize( obj.internal_getXkk().size() + FEAT_SIZE );
1116 
1117  for (q=0;q<FEAT_SIZE;q++)
1118  obj.internal_getXkk()[idx+q] = yn[q];
1119 
1120  // --------------------
1121  // Append to Pkk:
1122  // --------------------
1123  ASSERTDEB_( obj.internal_getPkk().getColCount()==idx && obj.internal_getPkk().getRowCount()==idx );
1124 
1125  obj.internal_getPkk().setSize( idx+FEAT_SIZE,idx+FEAT_SIZE );
1126 
1127  // Fill the Pxyn term:
1128  // --------------------
1129  typename KF::KFMatrix_VxV Pxx;
1130  obj.internal_getPkk().extractMatrix(0,0,Pxx);
1131  typename KF::KFMatrix_FxV Pxyn; // Pxyn = dyn_dxv * Pxx
1132  Pxyn.multiply( dyn_dxv, Pxx );
1133 
1134  obj.internal_getPkk().insertMatrix( idx,0, Pxyn );
1135  obj.internal_getPkk().insertMatrixTranspose( 0,idx, Pxyn );
1136 
1137  // Fill the Pyiyn terms:
1138  // --------------------
1139  const size_t nLMs = (idx-VEH_SIZE)/FEAT_SIZE; // Number of previous landmarks:
1140  for (q=0;q<nLMs;q++)
1141  {
1142  typename KF::KFMatrix_VxF P_x_yq(mrpt::math::UNINITIALIZED_MATRIX);
1143  obj.internal_getPkk().extractMatrix(0,VEH_SIZE+q*FEAT_SIZE,P_x_yq) ;
1144 
1145  typename KF::KFMatrix_FxF P_cross(mrpt::math::UNINITIALIZED_MATRIX);
1146  P_cross.multiply(dyn_dxv, P_x_yq );
1147 
1148  obj.internal_getPkk().insertMatrix(idx,VEH_SIZE+q*FEAT_SIZE, P_cross );
1149  obj.internal_getPkk().insertMatrixTranspose(VEH_SIZE+q*FEAT_SIZE,idx, P_cross );
1150  } // end each previous LM(q)
1151 
1152  // Fill the Pynyn term:
1153  // P_yn_yn = (dyn_dxv * Pxx * ~dyn_dxv) + (dyn_dhn * R * ~dyn_dhn);
1154  // --------------------
1155  typename KF::KFMatrix_FxF P_yn_yn(mrpt::math::UNINITIALIZED_MATRIX);
1156  dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
1157  if (use_dyn_dhn_jacobian)
1158  dyn_dhn.multiply_HCHt(R, P_yn_yn, true); // Accumulate in P_yn_yn
1159  else P_yn_yn+=dyn_dhn_R_dyn_dhnT;
1160 
1161  obj.internal_getPkk().insertMatrix(idx,idx, P_yn_yn );
1162 
1163  obj.getProfiler().leave("KF:9.create new LMs");
1164  }
1165  }
1166  }
1167 
1168  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1172  const vector_int &data_association,
1174  )
1175  {
1176  // Do nothing: this is NOT a SLAM problem.
1177  }
1178 
1179  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1181  {
1182  return (obj.getStateVectorLength()-VEH_SIZE)/FEAT_SIZE;
1183  }
1184  // Specialization for FEAT_SIZE=0
1185  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1187  {
1188  return 0;
1189  }
1190 
1191  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1193  {
1194  return (obj.getStateVectorLength()==VEH_SIZE);
1195  }
1196  // Specialization for FEAT_SIZE=0
1197  template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1199  {
1200  return true;
1201  }
1202  } // end namespace "detail"
1203  } // ns
1204 } // ns
1205 
1206 #endif
mrpt::utils::CTimeLogger & getProfiler()
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".
#define THROW_EXCEPTION(msg)
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.
A numeric matrix of compile-time fixed size.
#define MRPT_END
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)
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.
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...
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)
T abs(T x)
Definition: nanoflann.hpp:237
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.
double kftype
The numeric type used in the Kalman Filter (default=double)
#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.
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...
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
#define ASSERT_(f)
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:82
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
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:77
#define ASSERTMSG_(f, __ERROR_MSG)
Eigen::Matrix< double, Eigen::Dynamic, 1 > KFVector
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.12 for MRPT 1.3.2 SVN: at Thu Nov 10 13:22:34 UTC 2016