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);
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 MRPT_LOG_WARN_STREAM(
"[KF] *Performance Warning*: " <<nNew <<
" LMs were not correctly predicted by OnPreComputingPredictions().");
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;
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 bind1st(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;
575 OnObservationModel(
vector_size_t(1,idxInTheFilter),pred_obs);
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!")
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;
725 if (nKF_iterations>1)
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(
812 R.extractMatrix(R_row_offset,0, Si);
828 KFMatrix Pyix( FEAT_SIZE, VEH_SIZE );
829 m_pkk.extractMatrix(idx_off,0, Pyix);
831 term.multiply_ABCt( Hy, Pyix, Hx );
848 size_t N = m_pkk.getColCount();
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");
1005 static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
1016 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1019 const std::pair<KFCLASS*,KFArray_ACT> &dat,
1024 dat.first->OnTransitionModel(dat.second,out_x, dumm);
1027 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1030 const std::pair<KFCLASS*,size_t> &dat,
1036 ::memcpy(&dat.first->m_xkk[0],&
x[0],
sizeof(
x[0])*VEH_SIZE);
1037 dat.first->OnObservationModel(idxs_to_predict,prediction);
1039 out_x=prediction[0];
1042 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1045 const std::pair<KFCLASS*,size_t> &dat,
1050 const size_t lm_idx_in_statevector = VEH_SIZE+FEAT_SIZE*dat.second;
1052 ::memcpy(&dat.first->m_xkk[lm_idx_in_statevector],&
x[0],
sizeof(
x[0])*FEAT_SIZE);
1053 dat.first->OnObservationModel(idxs_to_predict,prediction);
1055 out_x=prediction[0];
1062 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1072 for (
size_t idxObs=0;idxObs<Z.size();idxObs++)
1075 if ( data_association[idxObs] < 0 )
1082 ASSERTDEB_( 0 == ((
obj.internal_getXkk().size() - VEH_SIZE) % FEAT_SIZE) )
1084 const size_t newIndexInMap = (
obj.internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
1087 typename KF::KFArray_FEAT yn;
1088 typename KF::KFMatrix_FxV dyn_dxv;
1089 typename KF::KFMatrix_FxO dyn_dhn;
1090 typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1091 bool use_dyn_dhn_jacobian=
true;
1094 obj.OnInverseObservationModel(
1100 use_dyn_dhn_jacobian );
1103 obj.OnNewLandmarkAddedToMap(
1112 size_t idx =
obj.internal_getXkk().size();
1113 obj.internal_getXkk().conservativeResize(
obj.internal_getXkk().size() + FEAT_SIZE );
1115 for (
q=0;
q<FEAT_SIZE;
q++)
1116 obj.internal_getXkk()[idx+
q] = yn[
q];
1121 ASSERTDEB_(
obj.internal_getPkk().getColCount()==idx &&
obj.internal_getPkk().getRowCount()==idx );
1123 obj.internal_getPkk().setSize( idx+FEAT_SIZE,idx+FEAT_SIZE );
1127 typename KF::KFMatrix_VxV Pxx;
1128 obj.internal_getPkk().extractMatrix(0,0,Pxx);
1129 typename KF::KFMatrix_FxV Pxyn;
1130 Pxyn.multiply( dyn_dxv, Pxx );
1132 obj.internal_getPkk().insertMatrix( idx,0, Pxyn );
1133 obj.internal_getPkk().insertMatrixTranspose( 0,idx, Pxyn );
1137 const size_t nLMs = (idx-VEH_SIZE)/FEAT_SIZE;
1138 for (
q=0;
q<nLMs;
q++)
1141 obj.internal_getPkk().extractMatrix(0,VEH_SIZE+
q*FEAT_SIZE,P_x_yq) ;
1144 P_cross.multiply(dyn_dxv, P_x_yq );
1146 obj.internal_getPkk().insertMatrix(idx,VEH_SIZE+
q*FEAT_SIZE, P_cross );
1147 obj.internal_getPkk().insertMatrixTranspose(VEH_SIZE+
q*FEAT_SIZE,idx, P_cross );
1154 dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
1155 if (use_dyn_dhn_jacobian)
1156 dyn_dhn.multiply_HCHt(
R, P_yn_yn,
true);
1157 else P_yn_yn+=dyn_dhn_R_dyn_dhnT;
1159 obj.internal_getPkk().insertMatrix(idx,idx, P_yn_yn );
1161 obj.getProfiler().leave(
"KF:9.create new LMs");
1166 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1177 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1180 return (
obj.getStateVectorLength()-VEH_SIZE)/FEAT_SIZE;
1183 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1189 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1192 return (
obj.getStateVectorLength()==VEH_SIZE);
1195 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
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".
mrpt::utils::CTimeLogger & getProfiler()
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
GLsizei GLsizei GLuint * obj
A numeric matrix of compile-time fixed size.
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
static void KF_aux_estimate_trans_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, KFArray_ACT > &dat, KFArray_VEH &out_x)
Auxiliary functions for Jacobian numeric estimation.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
#define MRPT_LOG_DEBUG(_STRING)
void addNewLandmarks(CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::vector_KFArray_OBS &Z, const vector_int &data_association, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO &R)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
double kftype
The numeric type used in the Kalman Filter (default=double)
GLsizei GLboolean transpose
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
GLdouble GLdouble GLdouble r
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
std::vector< size_t > vector_size_t
std::vector< int32_t > vector_int
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
void estimateJacobian(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
bool BASE_IMPEXP vectorToTextFile(const std::vector< float > &vec, const std::string &fileName, bool append=false, bool byRows=false)
A useful function for debugging, which saves a std::vector into a text file (compat.
size_t find_in_vector(const T &value, const CONTAINER &vect)
Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.
void enter(const char *func_name)
Start of a named section.
#define ASSERTMSG_(f, __ERROR_MSG)
GLubyte GLubyte GLubyte a
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Eigen::Matrix< double, Eigen::Dynamic, 1 > KFVector