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
std::vector< int32_t > vector_int
Definition: types_simple.h:23
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Tue Dec 27 00:54:45 UTC 2022