9 #ifndef CKalmanFilterCapable_IMPL_H
10 #define CKalmanFilterCapable_IMPL_H
12 #ifndef CKalmanFilterCapable_H
13 #error Include this file only from 'CKalmanFilterCapable.h'
21 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
27 m_timLogger.enable(KF_options.enable_profiler || KF_options.verbose);
28 m_timLogger.enter(
"KF:complete_step");
30 ASSERT_(
size_t(m_xkk.size())==m_pkk.getColCount())
31 ASSERT_(
size_t(m_xkk.size())>=VEH_SIZE)
38 m_timLogger.enter(
"KF:1.OnGetAction");
40 m_timLogger.leave(
"KF:1.OnGetAction");
43 if (FEAT_SIZE) {
ASSERTDEB_( (((m_xkk.size()-VEH_SIZE)/FEAT_SIZE)*FEAT_SIZE)== (m_xkk.size()-VEH_SIZE) ) }
48 m_timLogger.enter(
"KF:2.prediction stage");
50 const size_t N_map = getNumberOfLandmarksInTheMap();
54 bool skipPrediction=
false;
58 OnTransitionModel(u, xv, skipPrediction);
60 if ( !skipPrediction )
69 m_user_didnt_implement_jacobian=
false;
70 if (KF_options.use_analytic_transition_jacobian)
71 OnTransitionJacobian(dfv_dxv);
73 if (m_user_didnt_implement_jacobian || !KF_options.use_analytic_transition_jacobian || KF_options.debug_verify_analytic_jacobians)
77 OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
81 &KF_aux_estimate_trans_jacobian,
83 std::pair<KFCLASS*,KFArray_ACT>(
this,u),
86 if (KF_options.debug_verify_analytic_jacobians)
89 OnTransitionJacobian(dfv_dxv_gt);
90 if ((dfv_dxv-dfv_dxv_gt).array().
abs().
sum()>KF_options.debug_verify_analytic_jacobians_threshold)
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)")
102 OnTransitionNoise(Q);
108 Eigen::Block<typename KFMatrix::Base,VEH_SIZE,VEH_SIZE>(m_pkk,0,0) =
110 dfv_dxv * Eigen::Block<typename KFMatrix::Base,VEH_SIZE,VEH_SIZE>(m_pkk,0,0) * dfv_dxv.transpose();
117 for (
size_t i=0 ; i<N_map ; i++)
119 aux = dfv_dxv * Eigen::Block<typename KFMatrix::Base,VEH_SIZE,FEAT_SIZE>(m_pkk,0,VEH_SIZE+i*FEAT_SIZE);
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();
128 for (
size_t i=0;i<VEH_SIZE;i++)
132 OnNormalizeStateVector();
136 const double tim_pred = m_timLogger.leave(
"KF:2.prediction stage");
141 m_timLogger.enter(
"KF:3.predict all obs");
144 OnGetObservationNoise(R);
148 all_predictions.resize(N_map);
150 mrpt::math::sequenceStdVec<size_t,1>(0,N_map),
153 const double tim_pred_obs = m_timLogger.leave(
"KF:3.predict all obs");
155 m_timLogger.enter(
"KF:4.decide pred obs");
158 predictLMidxs.clear();
161 predictLMidxs.assign(1,0);
164 OnPreComputingPredictions(all_predictions, predictLMidxs);
167 m_timLogger.leave(
"KF:4.decide pred obs");
189 m_timLogger.enter(
"KF:5.build Jacobians");
191 size_t N_pred = FEAT_SIZE==0 ?
193 predictLMidxs.size();
199 std::vector<size_t> missing_predictions_to_add;
204 size_t first_new_pred = 0;
208 if (!missing_predictions_to_add.empty())
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));
214 for (
size_t j=0;j<nNew;j++)
215 predictLMidxs.push_back( missing_predictions_to_add[j] );
217 N_pred = predictLMidxs.size();
218 missing_predictions_to_add.clear();
224 for (
size_t i=first_new_pred;i<N_pred;++i)
226 const size_t lm_idx = FEAT_SIZE==0 ? 0 : predictLMidxs[i];
231 m_user_didnt_implement_jacobian=
false;
232 if (KF_options.use_analytic_observation_jacobian)
233 OnObservationJacobians(lm_idx,Hx,Hy);
235 if (m_user_didnt_implement_jacobian || !KF_options.use_analytic_observation_jacobian || KF_options.debug_verify_analytic_jacobians)
237 const size_t lm_idx_in_statevector = VEH_SIZE+lm_idx*FEAT_SIZE;
240 const KFArray_FEAT x_feat( &m_xkk[lm_idx_in_statevector] );
244 OnObservationJacobiansNumericGetIncrements(xkk_veh_increments, feat_increments);
248 &KF_aux_estimate_obs_Hx_jacobian,
250 std::pair<KFCLASS*,size_t>(
this,lm_idx),
253 ::memcpy(&m_xkk[0],&x_vehicle[0],
sizeof(m_xkk[0])*VEH_SIZE);
257 &KF_aux_estimate_obs_Hy_jacobian,
259 std::pair<KFCLASS*,size_t>(
this,lm_idx),
262 ::memcpy(&m_xkk[lm_idx_in_statevector],&x_feat[0],
sizeof(m_xkk[0])*FEAT_SIZE);
264 if (KF_options.debug_verify_analytic_jacobians)
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)")
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)")
282 m_timLogger.leave(
"KF:5.build Jacobians");
284 m_timLogger.enter(
"KF:6.build S");
291 S.setSize(N_pred*OBS_SIZE,N_pred*OBS_SIZE);
295 const Eigen::Block<const typename KFMatrix::Base,VEH_SIZE,VEH_SIZE> Px(m_pkk,0,0);
297 for (
size_t i=0;i<N_pred;++i)
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);
303 for (
size_t j=i;j<N_pred;++j)
305 const size_t lm_idx_j = predictLMidxs[j];
307 Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE> Sij(S,OBS_SIZE*i,OBS_SIZE*j);
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);
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();
319 Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(S,OBS_SIZE*j,OBS_SIZE*i) = Sij.transpose();
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;
332 S = Hxs[0] * m_pkk *Hxs[0].transpose() + R;
335 m_timLogger.leave(
"KF:6.build S");
339 m_timLogger.enter(
"KF:7.get obs & DA");
342 OnGetObservationsAndDataAssociation(
344 all_predictions, S, predictLMidxs, R
346 ASSERTDEB_(data_association.size()==Z.size() || (data_association.empty() && FEAT_SIZE==0));
351 missing_predictions_to_add.clear();
354 for (
size_t i=0;i<data_association.size();++i)
356 if (data_association[i]<0)
continue;
357 const size_t assoc_idx_in_map =
static_cast<size_t>(data_association[i]);
359 if (assoc_idx_in_pred==std::string::npos)
360 missing_predictions_to_add.push_back(assoc_idx_in_map);
364 first_new_pred = N_pred;
366 }
while (!missing_predictions_to_add.empty());
369 const double tim_obs_DA = m_timLogger.leave(
"KF:7.get obs & DA");
377 m_timLogger.enter(
"KF:8.update stage");
379 switch (KF_options.method)
390 vector_int mapIndicesForKFUpdate(data_association.size());
391 mapIndicesForKFUpdate.resize(
std::distance(mapIndicesForKFUpdate.begin(),
393 data_association.begin(),
394 data_association.end(),
395 mapIndicesForKFUpdate.begin(),
396 binder1st<equal_to<int> >(equal_to<int>(),-1) ) ) );
398 const size_t N_upd = (FEAT_SIZE==0) ?
400 mapIndicesForKFUpdate.size();
403 const size_t nKF_iterations = (KF_options.method==
kfEKFNaive) ? 1 : KF_options.IKF_iterations;
410 for (
size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
414 size_t ytilde_idx = 0;
417 dh_dx_full_obs.zeros(N_upd*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_map );
423 S_idxs.reserve(OBS_SIZE*N_upd);
427 for (
size_t i=0;i<data_association.size();++i)
429 if (data_association[i]<0)
continue;
431 const size_t assoc_idx_in_map =
static_cast<size_t>(data_association[i]);
433 ASSERTMSG_(assoc_idx_in_pred!=string::npos,
"OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!")
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];
445 for (
size_t k=0;k<OBS_SIZE;k++)
446 S_idxs.push_back(assoc_idx_in_pred*OBS_SIZE+k);
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];
455 S.extractSubmatrixSymmetrical(S_idxs,S_observed);
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)
463 dh_dx_full_obs = Hxs[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];
474 m_timLogger.enter(
"KF:8.update stage:1.FULLKF:build K");
476 K.setSize(m_pkk.getRowCount(), S_observed.getColCount() );
479 K.multiply_ABt(m_pkk, dh_dx_full_obs);
485 m_timLogger.leave(
"KF:8.update stage:1.FULLKF:build K");
488 if (nKF_iterations==1)
490 m_timLogger.enter(
"KF:8.update stage:2.FULLKF:update xkk");
492 m_timLogger.leave(
"KF:8.update stage:2.FULLKF:update xkk");
496 m_timLogger.enter(
"KF:8.update stage:2.FULLKF:iter.update xkk");
499 dh_dx_full_obs.multiply_Ab( m_xkk - xkk_0, HAx_column);
508 m_timLogger.leave(
"KF:8.update stage:2.FULLKF:iter.update xkk");
513 if (IKF_iteration == (nKF_iterations-1) )
515 m_timLogger.enter(
"KF:8.update stage:3.FULLKF:update Pkk");
522 aux_K_dh_dx.multiply(K,dh_dx_full_obs);
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++) {
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);
534 m_pkk.multiply_result_is_symmetric(aux_K_dh_dx, m_pkk );
536 m_timLogger.leave(
"KF:8.update stage:3.FULLKF:update Pkk");
549 for (
size_t obsIdx=0;obsIdx<Z.size();obsIdx++)
553 size_t idxInTheFilter=0;
555 if (data_association.empty())
561 doit = data_association[obsIdx] >= 0;
563 idxInTheFilter = data_association[obsIdx];
568 m_timLogger.enter(
"KF:8.update stage:1.ScalarAtOnce.prepare");
571 const size_t idx_off = VEH_SIZE + idxInTheFilter*FEAT_SIZE;
574 vector_KFArray_OBS pred_obs;
575 OnObservationModel(
vector_size_t(1,idxInTheFilter),pred_obs);
579 KFArray_OBS ytilde = Z[obsIdx];
580 OnSubstractObservationVectors(ytilde, pred_obs[0]);
587 ASSERTMSG_(i_idx_in_preds!=string::npos,
"OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!")
589 const KFMatrix_OxV & Hx = Hxs[i_idx_in_preds];
590 const KFMatrix_OxF & Hy = Hys[i_idx_in_preds];
592 m_timLogger.leave("KF:8.update stage:1.ScalarAtOnce.prepare");
595 for (
size_t j=0;j<OBS_SIZE;j++)
597 m_timLogger.enter(
"KF:8.update stage:2.ScalarAtOnce.update");
614 for (
size_t a=0;a<OBS_SIZE;a++)
615 for (
size_t b=0;b<OBS_SIZE;b++)
618 THROW_EXCEPTION(
"This KF algorithm assumes independent noise components in the observation (matrix R). Select another KF algorithm.")
622 KFTYPE Sij = R.get_unsafe(j,j);
625 for (
size_t k=0;k<VEH_SIZE;k++)
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;
635 for (
size_t k=0;k<VEH_SIZE;k++)
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;
645 for (
size_t k=0;k<FEAT_SIZE;k++)
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;
655 size_t N = m_pkk.getColCount();
656 vector<KFTYPE> Kij( N );
658 for (
size_t k=0;k<N;k++)
664 for (q=0;q<VEH_SIZE;q++)
665 K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(j,q);
668 for (q=0;q<FEAT_SIZE;q++)
669 K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(j,q);
671 Kij[k] = K_tmp / Sij;
677 for (
size_t k=0;k<N;k++)
678 m_xkk[k] += Kij[k] * ytilde[j];
683 for (
size_t k=0;k<N;k++)
685 for (
size_t q=k;q<N;q++)
687 m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
689 m_pkk(q,k) = m_pkk(k,q);
692 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
695 m_pkk.saveToTextFile(
"Pkk_err.txt");
704 m_timLogger.leave(
"KF:8.update stage:2.ScalarAtOnce.update");
721 size_t nKF_iterations = KF_options.IKF_iterations;
724 KFMatrix *saved_Pkk=NULL;
725 if (nKF_iterations>1)
728 saved_Pkk =
new KFMatrix( m_pkk );
731 KFVector xkk_0 = m_xkk;
732 KFVector xkk_next_iter = m_xkk;
735 for (
size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
743 xkk_next_iter = xkk_0;
747 for (
size_t obsIdx=0;obsIdx<Z.getRowCount();obsIdx++)
751 size_t idxInTheFilter=0;
753 if (data_association.empty())
759 doit = data_association[obsIdx] >= 0;
761 idxInTheFilter = data_association[obsIdx];
767 const size_t idx_off = VEH_SIZE + idxInTheFilter *FEAT_SIZE;
768 const size_t R_row_offset = obsIdx*OBS_SIZE;
772 OnObservationModelAndJacobians(
811 KFMatrix Si(OBS_SIZE,OBS_SIZE);
812 R.extractMatrix(R_row_offset,0, Si);
815 KFMatrix term(OBS_SIZE,OBS_SIZE);
828 KFMatrix Pyix( FEAT_SIZE, VEH_SIZE );
829 m_pkk.extractMatrix(idx_off,0, Pyix);
831 term.multiply_ABCt( Hy, Pyix, Hx );
844 KFMatrix Si_1(OBS_SIZE,OBS_SIZE);
848 size_t N = m_pkk.getColCount();
850 KFMatrix Ki( N, OBS_SIZE );
856 for (
size_t c=0;c<OBS_SIZE;c++)
861 for (q=0;q<VEH_SIZE;q++)
862 K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(c,q);
865 for (q=0;q<FEAT_SIZE;q++)
866 K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(c,q);
868 Ki.set_unsafe(k,c, K_tmp);
872 Ki.multiply(Ki, Si.inv() );
876 if (nKF_iterations==1)
881 for (
size_t q=0;q<OBS_SIZE;q++)
882 m_xkk[k] += Ki.get_unsafe(k,q) * ytilde[q];
887 Eigen::Matrix<KFTYPE,Eigen::Dynamic,1> HAx(OBS_SIZE);
890 for (o=0;o<OBS_SIZE;o++)
893 for (q=0;q<VEH_SIZE;q++)
894 tmp += Hx.get_unsafe(o,q) * (xkk_0[q] - m_xkk[q]);
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]);
903 for (o=0;o<OBS_SIZE;o++)
904 HAx[o] = ytilde[o] - HAx[o];
910 KFTYPE tmp = xkk_next_iter[k];
912 for (o=0;o<OBS_SIZE;o++)
913 tmp += Ki.get_unsafe(k,o) * HAx[o];
915 xkk_next_iter[k] = tmp;
924 Ki.multiplyByMatrixAndByTransposeNonSymmetric(
930 m_pkk.force_symmetry();
958 if (nKF_iterations>1)
961 cout <<
"IKF iter: " << IKF_iteration <<
" -> " << xkk_next_iter << endl;
963 m_xkk = xkk_next_iter;
969 if (saved_Pkk)
delete saved_Pkk;
981 const double tim_update = m_timLogger.leave(
"KF:8.update stage");
983 m_timLogger.enter(
"KF:9.OnNormalizeStateVector");
984 OnNormalizeStateVector();
985 m_timLogger.leave(
"KF:9.OnNormalizeStateVector");
990 if (!data_association.empty())
992 m_timLogger.enter(
"KF:A.add new landmarks");
994 m_timLogger.leave(
"KF:A.add new landmarks");
998 m_timLogger.enter(
"KF:B.OnPostIteration");
1000 m_timLogger.leave(
"KF:B.OnPostIteration");
1002 m_timLogger.leave(
"KF:complete_step");
1004 if (KF_options.verbose)
1006 printf_debug(
"[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: %.2fms\n",
1007 static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
1020 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1023 const std::pair<KFCLASS*,KFArray_ACT> &dat,
1028 dat.first->OnTransitionModel(dat.second,out_x, dumm);
1031 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1034 const std::pair<KFCLASS*,size_t> &dat,
1040 ::memcpy(&dat.first->m_xkk[0],&x[0],
sizeof(x[0])*VEH_SIZE);
1041 dat.first->OnObservationModel(idxs_to_predict,prediction);
1043 out_x=prediction[0];
1046 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1049 const std::pair<KFCLASS*,size_t> &dat,
1054 const size_t lm_idx_in_statevector = VEH_SIZE+FEAT_SIZE*dat.second;
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);
1059 out_x=prediction[0];
1066 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1076 for (
size_t idxObs=0;idxObs<Z.size();idxObs++)
1079 if ( data_association[idxObs] < 0 )
1088 const size_t newIndexInMap = (obj.
internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
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;
1104 use_dyn_dhn_jacobian );
1119 for (q=0;q<FEAT_SIZE;q++)
1131 typename KF::KFMatrix_VxV Pxx;
1133 typename KF::KFMatrix_FxV Pxyn;
1134 Pxyn.multiply( dyn_dxv, Pxx );
1141 const size_t nLMs = (idx-VEH_SIZE)/FEAT_SIZE;
1142 for (q=0;q<nLMs;q++)
1148 P_cross.multiply(dyn_dxv, P_x_yq );
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 );
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);
1161 else P_yn_yn+=dyn_dhn_R_dyn_dhnT;
1170 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1181 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1187 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1193 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1199 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
mrpt::utils::CTimeLogger & getProfiler()
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
KFMatrix & internal_getPkk()
KFVector & internal_getXkk()
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.
size_t getStateVectorLength() const
Eigen::Matrix< KFTYPE, Eigen::Dynamic, 1 > KFVector
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,...
A numeric matrix of compile-time fixed size.
A matrix of dynamic size.
void enter(const char *func_name)
Start of a named section.
double leave(const char *func_name)
End of a named section.
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
std::vector< int32_t > vector_int
std::vector< size_t > vector_size_t
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 ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
#define THROW_EXCEPTION(msg)
#define ASSERTMSG_(f, __ERROR_MSG)
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...
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.