11 #ifndef CKalmanFilterCapable_H 12 #error Include this file only from 'CKalmanFilterCapable.h' 17 #include <Eigen/Dense> 25 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
27 void CKalmanFilterCapable<
28 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::runOneKalmanIteration()
33 m_timLogger.enable(KF_options.enable_profiler);
34 m_timLogger.enter(
"KF:complete_step");
36 ASSERT_(
int(m_xkk.size()) == m_pkk.cols());
37 ASSERT_(
size_t(m_xkk.size()) >= VEH_SIZE);
43 m_timLogger.enter(
"KF:1.OnGetAction");
45 m_timLogger.leave(
"KF:1.OnGetAction");
51 (((m_xkk.size() - VEH_SIZE) / FEAT_SIZE) * FEAT_SIZE) ==
52 (m_xkk.size() - VEH_SIZE));
58 m_timLogger.enter(
"KF:2.prediction stage");
60 const size_t N_map = getNumberOfLandmarksInTheMap();
65 bool skipPrediction =
false;
73 OnTransitionModel(u, xv, skipPrediction);
85 m_user_didnt_implement_jacobian =
false;
88 if (KF_options.use_analytic_transition_jacobian)
89 OnTransitionJacobian(dfv_dxv);
91 if (m_user_didnt_implement_jacobian ||
92 !KF_options.use_analytic_transition_jacobian ||
93 KF_options.debug_verify_analytic_jacobians)
98 OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
104 const std::pair<KFCLASS*, KFArray_ACT>& dat,
105 KFArray_VEH& out_x)>(&KF_aux_estimate_trans_jacobian),
106 xkk_veh_increments, std::pair<KFCLASS*, KFArray_ACT>(
this, u),
109 if (KF_options.debug_verify_analytic_jacobians)
112 OnTransitionJacobian(dfv_dxv_gt);
113 if ((dfv_dxv - dfv_dxv_gt).sum_abs() >
114 KF_options.debug_verify_analytic_jacobians_threshold)
116 std::cerr <<
"[KalmanFilter] ERROR: User analytical " 117 "transition Jacobians are wrong: \n" 118 <<
" Real dfv_dxv: \n" 119 << dfv_dxv <<
"\n Analytical dfv_dxv:\n" 120 << dfv_dxv_gt <<
"Diff:\n" 121 << (dfv_dxv - dfv_dxv_gt) <<
"\n";
123 "ERROR: User analytical transition Jacobians are wrong " 124 "(More details dumped to cerr)");
132 OnTransitionNoise(Q);
138 m_pkk.asEigen().template block<VEH_SIZE, VEH_SIZE>(0, 0) =
140 m_pkk.template block<VEH_SIZE, VEH_SIZE>(0, 0) *
148 for (
size_t i = 0; i < N_map; i++)
150 aux = dfv_dxv.
asEigen() * m_pkk.template block<VEH_SIZE, FEAT_SIZE>(
151 0, VEH_SIZE + i * FEAT_SIZE);
153 m_pkk.asEigen().template block<VEH_SIZE, FEAT_SIZE>(
154 0, VEH_SIZE + i * FEAT_SIZE) = aux.
asEigen();
155 m_pkk.asEigen().template block<FEAT_SIZE, VEH_SIZE>(
156 VEH_SIZE + i * FEAT_SIZE, 0) = aux.
asEigen().transpose();
162 for (
size_t i = 0; i < VEH_SIZE; i++) m_xkk[i] = xv[i];
165 OnNormalizeStateVector();
169 const double tim_pred = m_timLogger.leave(
"KF:2.prediction stage");
174 m_timLogger.enter(
"KF:3.predict all obs");
177 OnGetObservationNoise(
R);
181 m_all_predictions.resize(N_map);
183 mrpt::math::sequenceStdVec<size_t, 1>(0, N_map), m_all_predictions);
185 const double tim_pred_obs = m_timLogger.leave(
"KF:3.predict all obs");
187 m_timLogger.enter(
"KF:4.decide pred obs");
190 m_predictLMidxs.clear();
194 m_predictLMidxs.assign(1, 0);
197 OnPreComputingPredictions(m_all_predictions, m_predictLMidxs);
199 m_timLogger.leave(
"KF:4.decide pred obs");
222 m_timLogger.enter(
"KF:5.build Jacobians");
227 : m_predictLMidxs.size();
237 std::vector<size_t> missing_predictions_to_add;
239 m_Hxs.resize(N_pred);
240 m_Hys.resize(N_pred);
242 size_t first_new_pred = 0;
248 if (!missing_predictions_to_add.empty())
250 const size_t nNew = missing_predictions_to_add.size();
252 "[KF] *Performance Warning*: " 254 <<
" LMs were not correctly predicted by " 255 "OnPreComputingPredictions().");
258 for (
size_t j = 0; j < nNew; j++)
259 m_predictLMidxs.push_back(missing_predictions_to_add[j]);
261 N_pred = m_predictLMidxs.size();
262 missing_predictions_to_add.clear();
265 m_Hxs.resize(N_pred);
266 m_Hys.resize(N_pred);
268 for (
size_t i = first_new_pred; i < N_pred; ++i)
270 const size_t lm_idx = FEAT_SIZE == 0 ? 0 : m_predictLMidxs[i];
275 m_user_didnt_implement_jacobian =
278 if (KF_options.use_analytic_observation_jacobian)
279 OnObservationJacobians(lm_idx, Hx, Hy);
281 if (m_user_didnt_implement_jacobian ||
282 !KF_options.use_analytic_observation_jacobian ||
283 KF_options.debug_verify_analytic_jacobians)
285 const size_t lm_idx_in_statevector =
286 VEH_SIZE + lm_idx * FEAT_SIZE;
289 const KFArray_FEAT x_feat(&m_xkk[lm_idx_in_statevector]);
293 OnObservationJacobiansNumericGetIncrements(
294 xkk_veh_increments, feat_increments);
300 const std::pair<KFCLASS*, size_t>& dat,
301 KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hx_jacobian),
303 std::pair<KFCLASS*, size_t>(
this, lm_idx), Hx);
307 &m_xkk[0], &x_vehicle[0],
sizeof(m_xkk[0]) * VEH_SIZE);
313 const std::pair<KFCLASS*, size_t>& dat,
314 KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hy_jacobian),
315 feat_increments, std::pair<KFCLASS*, size_t>(
this, lm_idx),
320 &m_xkk[lm_idx_in_statevector], &x_feat[0],
321 sizeof(m_xkk[0]) * FEAT_SIZE);
323 if (KF_options.debug_verify_analytic_jacobians)
327 OnObservationJacobians(lm_idx, Hx_gt, Hy_gt);
328 if (
KFMatrix(Hx - Hx_gt).sum_abs() >
329 KF_options.debug_verify_analytic_jacobians_threshold)
331 std::cerr <<
"[KalmanFilter] ERROR: User analytical " 332 "observation Hx Jacobians are wrong: \n" 334 << Hx.
asEigen() <<
"\n Analytical Hx:\n" 335 << Hx_gt.
asEigen() <<
"Diff:\n" 338 "ERROR: User analytical observation Hx Jacobians " 339 "are wrong (More details dumped to cerr)");
341 if (
KFMatrix(Hy - Hy_gt).sum_abs() >
342 KF_options.debug_verify_analytic_jacobians_threshold)
344 std::cerr <<
"[KalmanFilter] ERROR: User analytical " 345 "observation Hy Jacobians are wrong: \n" 347 << Hy.
asEigen() <<
"\n Analytical Hx:\n" 348 << Hy_gt.
asEigen() <<
"Diff:\n" 351 "ERROR: User analytical observation Hy Jacobians " 352 "are wrong (More details dumped to cerr)");
357 m_timLogger.leave(
"KF:5.build Jacobians");
359 m_timLogger.enter(
"KF:6.build m_S");
366 m_S.setSize(N_pred * OBS_SIZE, N_pred * OBS_SIZE);
371 const auto Px = m_pkk.template block<VEH_SIZE, VEH_SIZE>(0, 0);
373 for (
size_t i = 0; i < N_pred; ++i)
375 const size_t lm_idx_i = m_predictLMidxs[i];
377 const auto Pxyi_t = m_pkk.template block<FEAT_SIZE, VEH_SIZE>(
378 VEH_SIZE + lm_idx_i * FEAT_SIZE, 0);
381 for (
size_t j = i; j < N_pred; ++j)
383 const size_t lm_idx_j = m_predictLMidxs[j];
387 const auto Pxyj = m_pkk.template block<VEH_SIZE, FEAT_SIZE>(
388 0, VEH_SIZE + lm_idx_j * FEAT_SIZE);
390 m_pkk.template block<FEAT_SIZE, FEAT_SIZE>(
391 VEH_SIZE + lm_idx_i * FEAT_SIZE,
392 VEH_SIZE + lm_idx_j * FEAT_SIZE);
395 Sij = m_Hxs[i].
asEigen() * Px * m_Hxs[j].asEigen().transpose() +
396 m_Hys[i].asEigen() * Pxyi_t * m_Hxs[j].asEigen().transpose() +
397 m_Hxs[i].asEigen() * Pxyj * m_Hys[j].asEigen().transpose() +
398 m_Hys[i].asEigen() * Pyiyj * m_Hys[j].asEigen().transpose();
401 m_S.insertMatrix(OBS_SIZE * i, OBS_SIZE * j, Sij);
405 m_S.insertMatrixTransposed(
406 OBS_SIZE * j, OBS_SIZE * i, Sij);
410 const size_t obs_idx_off = i * OBS_SIZE;
411 m_S.asEigen().template block<OBS_SIZE, OBS_SIZE>(
412 obs_idx_off, obs_idx_off) +=
R.asEigen();
420 m_S = m_Hxs[0].asEigen() * m_pkk.asEigen() *
421 m_Hxs[0].asEigen().transpose() +
425 m_timLogger.leave(
"KF:6.build m_S");
429 m_timLogger.enter(
"KF:7.get obs & DA");
432 OnGetObservationsAndDataAssociation(
433 m_Z, data_association,
434 m_all_predictions, m_S, m_predictLMidxs,
R 437 data_association.size() == m_Z.size() ||
438 (data_association.empty() && FEAT_SIZE == 0));
445 missing_predictions_to_add.clear();
448 for (
int i : data_association)
451 const auto assoc_idx_in_map =
static_cast<size_t>(i);
452 const size_t assoc_idx_in_pred =
454 assoc_idx_in_map, m_predictLMidxs);
455 if (assoc_idx_in_pred == std::string::npos)
456 missing_predictions_to_add.push_back(assoc_idx_in_map);
460 first_new_pred = N_pred;
463 }
while (!missing_predictions_to_add.empty());
465 const double tim_obs_DA = m_timLogger.leave(
"KF:7.get obs & DA");
473 m_timLogger.enter(
"KF:8.update stage");
475 switch (KF_options.method)
486 std::vector<int> mapIndicesForKFUpdate(data_association.size());
488 mapIndicesForKFUpdate.begin(),
490 data_association.begin(), data_association.end(),
491 mapIndicesForKFUpdate.begin(),
492 [](
int i) {
return i == -1; })));
495 (FEAT_SIZE == 0) ? 1 :
498 mapIndicesForKFUpdate
502 const size_t nKF_iterations = (KF_options.method ==
kfEKFNaive)
504 : KF_options.IKF_iterations;
511 for (
size_t IKF_iteration = 0;
512 IKF_iteration < nKF_iterations; IKF_iteration++)
516 size_t ytilde_idx = 0;
520 m_dh_dx_full_obs.setZero(
522 VEH_SIZE + FEAT_SIZE * N_map);
529 std::vector<size_t> S_idxs;
530 S_idxs.reserve(OBS_SIZE * N_upd);
535 for (
size_t i = 0; i < data_association.size(); ++i)
537 if (data_association[i] < 0)
continue;
539 const auto assoc_idx_in_map =
540 static_cast<size_t>(data_association[i]);
541 const size_t assoc_idx_in_pred =
543 assoc_idx_in_map, m_predictLMidxs);
545 assoc_idx_in_pred != string::npos,
546 "OnPreComputingPredictions() didn't " 547 "recommend the prediction of a landmark " 548 "which has been actually observed!");
562 .template block<OBS_SIZE, VEH_SIZE>(
564 m_Hxs[assoc_idx_in_pred].asEigen();
566 .template block<OBS_SIZE, FEAT_SIZE>(
569 assoc_idx_in_map * FEAT_SIZE) =
570 m_Hys[assoc_idx_in_pred].asEigen();
574 for (
size_t k = 0; k < OBS_SIZE; k++)
576 assoc_idx_in_pred * OBS_SIZE + k);
580 OnSubstractObservationVectors(
583 [m_predictLMidxs[assoc_idx_in_pred]]);
584 for (
size_t k = 0; k < OBS_SIZE; k++)
585 ytilde[ytilde_idx++] = ytilde_i[k];
590 m_S, S_idxs, S_observed);
596 m_all_predictions.size() == 1);
598 m_dh_dx_full_obs.rows() == OBS_SIZE &&
599 m_dh_dx_full_obs.cols() == VEH_SIZE);
601 m_dh_dx_full_obs = m_Hxs[0];
603 OnSubstractObservationVectors(
604 ytilde_i, m_all_predictions[0]);
605 for (
size_t k = 0; k < OBS_SIZE; k++)
606 ytilde[ytilde_idx++] = ytilde_i[k];
614 m_timLogger.enter(
"KF:8.update stage:1.FULLKF:build K");
619 m_K.asEigen() = m_pkk.asEigen() *
620 m_dh_dx_full_obs.asEigen().transpose();
624 m_K.asEigen() *= m_S_1.asEigen();
626 m_timLogger.leave(
"KF:8.update stage:1.FULLKF:build K");
629 if (nKF_iterations == 1)
632 "KF:8.update stage:2.FULLKF:update xkk");
633 m_xkk.asEigen() += (m_K * ytilde).eval();
635 "KF:8.update stage:2.FULLKF:update xkk");
640 "KF:8.update stage:2.FULLKF:iter.update xkk");
643 KFVector(m_dh_dx_full_obs * (m_xkk - xkk_0));
646 m_xkk.asEigen() += m_K * (ytilde - HAx_column);
649 "KF:8.update stage:2.FULLKF:iter.update xkk");
655 if (IKF_iteration == (nKF_iterations - 1))
658 "KF:8.update stage:3.FULLKF:update Pkk");
665 m_aux_K_dh_dx.matProductOf_AB(
666 m_K, m_dh_dx_full_obs);
669 const size_t stat_len = m_aux_K_dh_dx.cols();
670 for (
size_t r = 0; r < stat_len; r++)
672 for (
size_t c = 0; c < stat_len; c++)
675 m_aux_K_dh_dx(r, c) =
676 -m_aux_K_dh_dx(r, c) +
kftype(1);
678 m_aux_K_dh_dx(r, c) =
679 -m_aux_K_dh_dx(r, c);
683 m_pkk = m_aux_K_dh_dx * m_pkk;
686 "KF:8.update stage:3.FULLKF:update Pkk");
699 for (
size_t obsIdx = 0; obsIdx < m_Z.size(); obsIdx++)
703 size_t idxInTheFilter = 0;
705 if (data_association.empty())
711 doit = data_association[obsIdx] >= 0;
712 if (doit) idxInTheFilter = data_association[obsIdx];
718 "KF:8.update stage:1.ScalarAtOnce.prepare");
721 const size_t idx_off =
730 std::vector<size_t>(1, idxInTheFilter), pred_obs);
735 OnSubstractObservationVectors(ytilde, pred_obs[0]);
742 const size_t i_idx_in_preds =
744 idxInTheFilter, m_predictLMidxs);
746 i_idx_in_preds != string::npos,
747 "OnPreComputingPredictions() didn't recommend the " 748 "prediction of a landmark which has been actually " 755 "KF:8.update stage:1.ScalarAtOnce.prepare");
758 for (
size_t j = 0; j < OBS_SIZE; j++)
761 "KF:8.update stage:2.ScalarAtOnce.update");
785 for (
size_t a = 0; a < OBS_SIZE; a++)
786 for (
size_t b = 0; b < OBS_SIZE; b++)
790 "This KF algorithm assumes " 793 "observation (matrix R). " 799 KFTYPE Sij =
R(j, j);
802 for (
size_t k = 0; k < VEH_SIZE; k++)
805 for (
size_t q = 0; q < VEH_SIZE; q++)
806 accum += Hx(j, q) * m_pkk(q, k);
807 Sij += Hx(j, k) * accum;
812 for (
size_t k = 0; k < VEH_SIZE; k++)
815 for (
size_t q = 0; q < FEAT_SIZE; q++)
816 accum += Hy(j, q) * m_pkk(idx_off + q, k);
817 term2 += Hx(j, k) * accum;
822 for (
size_t k = 0; k < FEAT_SIZE; k++)
825 for (
size_t q = 0; q < FEAT_SIZE; q++)
827 m_pkk(idx_off + q, idx_off + k);
828 Sij += Hy(j, k) * accum;
834 size_t N = m_pkk.
cols();
835 vector<KFTYPE> Kij(N);
837 for (
size_t k = 0; k < N; k++)
843 for (q = 0; q < VEH_SIZE; q++)
844 K_tmp += m_pkk(k, q) * Hx(j, q);
847 for (q = 0; q < FEAT_SIZE; q++)
848 K_tmp += m_pkk(k, idx_off + q) * Hy(j, q);
850 Kij[k] = K_tmp / Sij;
855 for (
size_t k = 0; k < N; k++)
856 m_xkk[k] += Kij[k] * ytilde[j];
861 for (
size_t k = 0; k < N; k++)
863 for (
size_t q = k; q < N;
866 m_pkk(k, q) -= Sij * Kij[k] * Kij[q];
868 m_pkk(q, k) = m_pkk(k, q);
871 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 874 m_pkk.saveToTextFile(
"Pkk_err.txt");
884 "KF:8.update stage:2.ScalarAtOnce.update");
905 const double tim_update = m_timLogger.leave(
"KF:8.update stage");
907 m_timLogger.enter(
"KF:9.OnNormalizeStateVector");
908 OnNormalizeStateVector();
909 m_timLogger.leave(
"KF:9.OnNormalizeStateVector");
914 if (!data_association.empty())
916 m_timLogger.enter(
"KF:A.add new landmarks");
918 m_timLogger.leave(
"KF:A.add new landmarks");
922 m_timLogger.enter(
"KF:B.OnPostIteration");
924 m_timLogger.leave(
"KF:B.OnPostIteration");
926 m_timLogger.leave(
"KF:complete_step");
929 "[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: " 931 static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
932 1e3 * tim_pred, 1e3 * tim_pred_obs, 1e3 * tim_obs_DA,
938 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
942 const KFArray_VEH& x,
const std::pair<KFCLASS*, KFArray_ACT>& dat,
947 dat.first->OnTransitionModel(dat.second, out_x, dumm);
951 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
955 const KFArray_VEH& x,
const std::pair<KFCLASS*, size_t>& dat,
958 std::vector<size_t> idxs_to_predict(1, dat.second);
961 std::memcpy(&dat.first->m_xkk[0], &x[0],
sizeof(x[0]) * VEH_SIZE);
962 dat.first->OnObservationModel(idxs_to_predict, prediction);
964 out_x = prediction[0];
968 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
972 const KFArray_FEAT& x,
const std::pair<KFCLASS*, size_t>& dat,
975 std::vector<size_t> idxs_to_predict(1, dat.second);
977 const size_t lm_idx_in_statevector = VEH_SIZE + FEAT_SIZE * dat.second;
980 &dat.first->m_xkk[lm_idx_in_statevector], &x[0],
981 sizeof(x[0]) * FEAT_SIZE);
982 dat.first->OnObservationModel(idxs_to_predict, prediction);
984 out_x = prediction[0];
991 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
996 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
997 const std::vector<int>& data_association,
999 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::KFMatrix_OxO&
R)
1004 for (
size_t idxObs = 0; idxObs < Z.size(); idxObs++)
1007 if (data_association[idxObs] < 0)
1018 const size_t newIndexInMap =
1022 typename KF::KFArray_FEAT yn;
1023 typename KF::KFMatrix_FxV dyn_dxv;
1024 typename KF::KFMatrix_FxO dyn_dhn;
1025 typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1026 bool use_dyn_dhn_jacobian =
true;
1030 Z[idxObs], yn, dyn_dxv, dyn_dhn, dyn_dhn_R_dyn_dhnT,
1031 use_dyn_dhn_jacobian);
1045 for (q = 0; q < FEAT_SIZE; q++)
1059 auto Pxx =
typename KF::KFMatrix_VxV(
1061 auto Pxyn =
typename KF::KFMatrix_FxV(dyn_dxv * Pxx);
1069 (idx - VEH_SIZE) / FEAT_SIZE;
1070 for (q = 0; q < nLMs; q++)
1072 auto P_x_yq =
typename KF::KFMatrix_VxF(
1074 0, VEH_SIZE + q * FEAT_SIZE));
1076 auto P_cross =
typename KF::KFMatrix_FxF(dyn_dxv * P_x_yq);
1079 idx, VEH_SIZE + q * FEAT_SIZE, P_cross);
1081 VEH_SIZE + q * FEAT_SIZE, idx, P_cross.transpose());
1088 typename KF::KFMatrix_FxF P_yn_yn =
1090 if (use_dyn_dhn_jacobian)
1094 P_yn_yn += dyn_dhn_R_dyn_dhnT;
1103 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1106 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>& obj,
1108 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
1109 KFTYPE>::vector_KFArray_OBS& Z,
1110 const std::vector<int>& data_association,
1112 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
1113 KFTYPE>::KFMatrix_OxO&
R)
1119 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1128 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1131 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>& obj)
1137 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1146 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1149 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>& obj)
A compile-time fixed-size numeric matrix container.
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
#define THROW_EXCEPTION(msg)
std::string std::string format(std::string_view fmt, ARGS &&... args)
size_t size(const MATRIXLIKE &m, const int dim)
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
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.
#define ASSERT_(f)
Defines an assertion mechanism.
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 std::vector< int > &data_association, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO &R)
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 ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
double kftype
The numeric type used in the Kalman Filter (default=double)
KFVector & internal_getXkk()
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, 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...
void extractSubmatrixSymmetrical(const MAT &m, const std::vector< size_t > &indices, MATRIX &out)
Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is the sequ...
void enter(const std::string_view &func_name) noexcept
Start of a named section.
std::vector< KFArray_OBS > vector_KFArray_OBS
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void OnNewLandmarkAddedToMap([[maybe_unused]] const size_t in_obsIdx, [[maybe_unused]] const size_t in_idxNewFeat)
If applicable to the given problem, do here any special handling of adding a new landmark to the map...
size_t getStateVectorLength() const
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
double leave(const std::string_view &func_name) noexcept
End of a named section.
virtual void OnInverseObservationModel([[maybe_unused]] const KFArray_OBS &in_z, [[maybe_unused]] KFArray_FEAT &out_yn, [[maybe_unused]] KFMatrix_FxV &out_dyn_dxv, [[maybe_unused]] KFMatrix_FxO &out_dyn_dhn) const
If applicable to the given problem, this method implements the inverse observation model needed to ex...
constexpr size_type cols() const
Number of columns in the matrix.
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
void resize(std::size_t N, bool zeroNewElements=false)
KFMatrix & internal_getPkk()
bool 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.
mrpt::system::CTimeLogger & getProfiler()
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.