Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
16namespace 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 // =============================================================
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 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;
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 if (KF_options.verbose)
1005 {
1006 printf_debug("[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: %.2fms\n",
1007 static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
1008 1e3*tim_pred,
1009 1e3*tim_pred_obs,
1010 1e3*tim_obs_DA,
1011 1e3*tim_update
1012 );
1013 }
1014 MRPT_END
1015
1016 } // End of "runOneKalmanIteration"
1017
1018
1019
1020 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1022 const KFArray_VEH &x,
1023 const std::pair<KFCLASS*,KFArray_ACT> &dat,
1024 KFArray_VEH &out_x)
1025 {
1026 bool dumm;
1027 out_x=x;
1028 dat.first->OnTransitionModel(dat.second,out_x, dumm);
1029 }
1030
1031 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1033 const KFArray_VEH &x,
1034 const std::pair<KFCLASS*,size_t> &dat,
1035 KFArray_OBS &out_x)
1036 {
1037 vector_size_t idxs_to_predict(1,dat.second);
1038 vector_KFArray_OBS prediction;
1039 // Overwrite (temporarily!) the affected part of the state vector:
1040 ::memcpy(&dat.first->m_xkk[0],&x[0],sizeof(x[0])*VEH_SIZE);
1041 dat.first->OnObservationModel(idxs_to_predict,prediction);
1042 ASSERTDEB_(prediction.size()==1);
1043 out_x=prediction[0];
1044 }
1045
1046 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1048 const KFArray_FEAT &x,
1049 const std::pair<KFCLASS*,size_t> &dat,
1050 KFArray_OBS &out_x)
1051 {
1052 vector_size_t idxs_to_predict(1,dat.second);
1053 vector_KFArray_OBS prediction;
1054 const size_t lm_idx_in_statevector = VEH_SIZE+FEAT_SIZE*dat.second;
1055 // Overwrite (temporarily!) the affected part of the state vector:
1056 ::memcpy(&dat.first->m_xkk[lm_idx_in_statevector],&x[0],sizeof(x[0])*FEAT_SIZE);
1057 dat.first->OnObservationModel(idxs_to_predict,prediction);
1058 ASSERTDEB_(prediction.size()==1);
1059 out_x=prediction[0];
1060 }
1061
1062
1063 namespace detail
1064 {
1065 // generic version for SLAM. There is a speciation below for NON-SLAM problems.
1066 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1070 const vector_int &data_association,
1072 )
1073 {
1075
1076 for (size_t idxObs=0;idxObs<Z.size();idxObs++)
1077 {
1078 // Is already in the map?
1079 if ( data_association[idxObs] < 0 ) // Not in the map yet!
1080 {
1081 obj.getProfiler().enter("KF:9.create new LMs");
1082 // Add it:
1083
1084 // Append to map of IDs <-> position in the state vector:
1085 ASSERTDEB_(FEAT_SIZE>0)
1086 ASSERTDEB_( 0 == ((obj.internal_getXkk().size() - VEH_SIZE) % FEAT_SIZE) ) // Sanity test
1087
1088 const size_t newIndexInMap = (obj.internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
1089
1090 // Inverse sensor model:
1091 typename KF::KFArray_FEAT yn;
1092 typename KF::KFMatrix_FxV dyn_dxv;
1093 typename KF::KFMatrix_FxO dyn_dhn;
1094 typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1095 bool use_dyn_dhn_jacobian=true;
1096
1097 // Compute the inv. sensor model and its Jacobians:
1099 Z[idxObs],
1100 yn,
1101 dyn_dxv,
1102 dyn_dhn,
1103 dyn_dhn_R_dyn_dhnT,
1104 use_dyn_dhn_jacobian );
1105
1106 // And let the application do any special handling of adding a new feature to the map:
1108 idxObs,
1109 newIndexInMap
1110 );
1111
1112 ASSERTDEB_( yn.size() == FEAT_SIZE )
1113
1114 // Append to m_xkk:
1115 size_t q;
1116 size_t idx = obj.internal_getXkk().size();
1117 obj.internal_getXkk().conservativeResize( obj.internal_getXkk().size() + FEAT_SIZE );
1118
1119 for (q=0;q<FEAT_SIZE;q++)
1120 obj.internal_getXkk()[idx+q] = yn[q];
1121
1122 // --------------------
1123 // Append to Pkk:
1124 // --------------------
1125 ASSERTDEB_( obj.internal_getPkk().getColCount()==idx && obj.internal_getPkk().getRowCount()==idx );
1126
1127 obj.internal_getPkk().setSize( idx+FEAT_SIZE,idx+FEAT_SIZE );
1128
1129 // Fill the Pxyn term:
1130 // --------------------
1131 typename KF::KFMatrix_VxV Pxx;
1132 obj.internal_getPkk().extractMatrix(0,0,Pxx);
1133 typename KF::KFMatrix_FxV Pxyn; // Pxyn = dyn_dxv * Pxx
1134 Pxyn.multiply( dyn_dxv, Pxx );
1135
1136 obj.internal_getPkk().insertMatrix( idx,0, Pxyn );
1137 obj.internal_getPkk().insertMatrixTranspose( 0,idx, Pxyn );
1138
1139 // Fill the Pyiyn terms:
1140 // --------------------
1141 const size_t nLMs = (idx-VEH_SIZE)/FEAT_SIZE; // Number of previous landmarks:
1142 for (q=0;q<nLMs;q++)
1143 {
1144 typename KF::KFMatrix_VxF P_x_yq(mrpt::math::UNINITIALIZED_MATRIX);
1145 obj.internal_getPkk().extractMatrix(0,VEH_SIZE+q*FEAT_SIZE,P_x_yq) ;
1146
1147 typename KF::KFMatrix_FxF P_cross(mrpt::math::UNINITIALIZED_MATRIX);
1148 P_cross.multiply(dyn_dxv, P_x_yq );
1149
1150 obj.internal_getPkk().insertMatrix(idx,VEH_SIZE+q*FEAT_SIZE, P_cross );
1151 obj.internal_getPkk().insertMatrixTranspose(VEH_SIZE+q*FEAT_SIZE,idx, P_cross );
1152 } // end each previous LM(q)
1153
1154 // Fill the Pynyn term:
1155 // P_yn_yn = (dyn_dxv * Pxx * ~dyn_dxv) + (dyn_dhn * R * ~dyn_dhn);
1156 // --------------------
1157 typename KF::KFMatrix_FxF P_yn_yn(mrpt::math::UNINITIALIZED_MATRIX);
1158 dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
1159 if (use_dyn_dhn_jacobian)
1160 dyn_dhn.multiply_HCHt(R, P_yn_yn, true); // Accumulate in P_yn_yn
1161 else P_yn_yn+=dyn_dhn_R_dyn_dhnT;
1162
1163 obj.internal_getPkk().insertMatrix(idx,idx, P_yn_yn );
1164
1165 obj.getProfiler().leave("KF:9.create new LMs");
1166 }
1167 }
1168 }
1169
1170 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1174 const vector_int &data_association,
1176 )
1177 {
1178 // Do nothing: this is NOT a SLAM problem.
1179 }
1180
1181 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1183 {
1184 return (obj.getStateVectorLength()-VEH_SIZE)/FEAT_SIZE;
1185 }
1186 // Specialization for FEAT_SIZE=0
1187 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1189 {
1190 return 0;
1191 }
1192
1193 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
1195 {
1196 return (obj.getStateVectorLength()==VEH_SIZE);
1197 }
1198 // Specialization for FEAT_SIZE=0
1199 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1201 {
1202 return true;
1203 }
1204 } // end namespace "detail"
1205 } // ns
1206} // ns
1207
1208#endif
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
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.
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
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.
Eigen::Matrix< KFTYPE, Eigen::Dynamic, 1 > KFVector
mrpt::utils::CTimeLogger & getProfiler()
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
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...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
Definition: CArrayNumeric.h:26
A numeric matrix of compile-time fixed size.
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:77
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:82
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
T abs(T x)
Definition: nanoflann.hpp:237
std::vector< int32_t > vector_int
Definition: types_simple.h:23
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
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".
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,...
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.
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: mrpt_macros.h:283
#define MRPT_END
Definition: mrpt_macros.h:353
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:110
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: mrpt_macros.h:260
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)
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
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
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.



Page generated by Doxygen 1.9.4 for MRPT 1.4.0 SVN: at Sun Aug 14 11:28:23 UTC 2022