9 #ifndef CKalmanFilterCapable_IMPL_H 10 #define CKalmanFilterCapable_IMPL_H 12 #ifndef CKalmanFilterCapable_H 13 #error Include this file only from 'CKalmanFilterCapable.h' 24 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
26 void CKalmanFilterCapable<
27 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::runOneKalmanIteration()
32 m_timLogger.enable(KF_options.enable_profiler);
33 m_timLogger.enter(
"KF:complete_step");
35 ASSERT_(
int(m_xkk.size()) == m_pkk.cols());
36 ASSERT_(
size_t(m_xkk.size()) >= VEH_SIZE);
42 m_timLogger.enter(
"KF:1.OnGetAction");
44 m_timLogger.leave(
"KF:1.OnGetAction");
50 (((m_xkk.size() - VEH_SIZE) / FEAT_SIZE) * FEAT_SIZE) ==
51 (m_xkk.size() - VEH_SIZE));
57 m_timLogger.enter(
"KF:2.prediction stage");
59 const size_t N_map = getNumberOfLandmarksInTheMap();
63 bool skipPrediction =
false;
71 OnTransitionModel(u, xv, skipPrediction);
83 m_user_didnt_implement_jacobian =
false;
86 if (KF_options.use_analytic_transition_jacobian)
87 OnTransitionJacobian(dfv_dxv);
89 if (m_user_didnt_implement_jacobian ||
90 !KF_options.use_analytic_transition_jacobian ||
91 KF_options.debug_verify_analytic_jacobians)
96 OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
102 const std::pair<KFCLASS*, KFArray_ACT>& dat,
103 KFArray_VEH& out_x)>(&KF_aux_estimate_trans_jacobian),
104 xkk_veh_increments, std::pair<KFCLASS*, KFArray_ACT>(
this, u),
107 if (KF_options.debug_verify_analytic_jacobians)
110 OnTransitionJacobian(dfv_dxv_gt);
111 if ((dfv_dxv - dfv_dxv_gt).array().abs().
sum() >
112 KF_options.debug_verify_analytic_jacobians_threshold)
114 std::cerr <<
"[KalmanFilter] ERROR: User analytical " 115 "transition Jacobians are wrong: \n" 116 <<
" Real dfv_dxv: \n" 117 << dfv_dxv <<
"\n Analytical dfv_dxv:\n" 118 << dfv_dxv_gt <<
"Diff:\n" 119 << (dfv_dxv - dfv_dxv_gt) <<
"\n";
121 "ERROR: User analytical transition Jacobians are wrong " 122 "(More details dumped to cerr)")
130 OnTransitionNoise(Q);
136 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, VEH_SIZE>(m_pkk, 0, 0) =
138 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, VEH_SIZE>(
147 for (
size_t i = 0; i < N_map; i++)
150 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>(
151 m_pkk, 0, VEH_SIZE + i * FEAT_SIZE);
153 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>(
154 m_pkk, 0, VEH_SIZE + i * FEAT_SIZE) = aux;
155 Eigen::Block<typename KFMatrix::Base, FEAT_SIZE, VEH_SIZE>(
156 m_pkk, VEH_SIZE + i * FEAT_SIZE, 0) = aux.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 all_predictions.resize(N_map);
183 mrpt::math::sequenceStdVec<size_t, 1>(0, N_map), 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 predictLMidxs.clear();
194 predictLMidxs.assign(1, 0);
197 OnPreComputingPredictions(all_predictions, predictLMidxs);
199 m_timLogger.leave(
"KF:4.decide pred obs");
222 m_timLogger.enter(
"KF:5.build Jacobians");
227 : predictLMidxs.size();
237 std::vector<size_t> missing_predictions_to_add;
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 predictLMidxs.push_back(missing_predictions_to_add[j]);
261 N_pred = predictLMidxs.size();
262 missing_predictions_to_add.clear();
268 for (
size_t i = first_new_pred; i < N_pred; ++i)
270 const size_t lm_idx = FEAT_SIZE == 0 ? 0 : 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);
306 ::memcpy(&m_xkk[0], &x_vehicle[0],
sizeof(m_xkk[0]) * VEH_SIZE);
312 const std::pair<KFCLASS*, size_t>& dat,
313 KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hy_jacobian),
314 feat_increments, std::pair<KFCLASS*, size_t>(
this, lm_idx),
319 &m_xkk[lm_idx_in_statevector], &x_feat[0],
320 sizeof(m_xkk[0]) * FEAT_SIZE);
322 if (KF_options.debug_verify_analytic_jacobians)
326 OnObservationJacobians(lm_idx, Hx_gt, Hy_gt);
327 if ((Hx - Hx_gt).array().abs().
sum() >
328 KF_options.debug_verify_analytic_jacobians_threshold)
330 std::cerr <<
"[KalmanFilter] ERROR: User analytical " 331 "observation Hx Jacobians are wrong: \n" 333 << Hx <<
"\n Analytical Hx:\n" 334 << Hx_gt <<
"Diff:\n" 335 << Hx - Hx_gt <<
"\n";
337 "ERROR: User analytical observation Hx Jacobians " 338 "are wrong (More details dumped to cerr)")
340 if ((Hy - Hy_gt).array().abs().
sum() >
341 KF_options.debug_verify_analytic_jacobians_threshold)
343 std::cerr <<
"[KalmanFilter] ERROR: User analytical " 344 "observation Hy Jacobians are wrong: \n" 346 << Hy <<
"\n Analytical Hx:\n" 347 << Hy_gt <<
"Diff:\n" 348 << Hy - Hy_gt <<
"\n";
350 "ERROR: User analytical observation Hy Jacobians " 351 "are wrong (More details dumped to cerr)")
356 m_timLogger.leave(
"KF:5.build Jacobians");
358 m_timLogger.enter(
"KF:6.build S");
365 S.setSize(N_pred * OBS_SIZE, N_pred * OBS_SIZE);
373 for (
size_t i = 0; i < N_pred; ++i)
375 const size_t lm_idx_i = predictLMidxs[i];
379 m_pkk, VEH_SIZE + lm_idx_i * FEAT_SIZE, 0);
382 for (
size_t j = i; j < N_pred; ++j)
384 const size_t lm_idx_j = predictLMidxs[j];
386 Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>
387 Sij(S, OBS_SIZE * i, OBS_SIZE * j);
391 Pxyj(m_pkk, 0, VEH_SIZE + lm_idx_j * FEAT_SIZE);
395 m_pkk, VEH_SIZE + lm_idx_i * FEAT_SIZE,
396 VEH_SIZE + lm_idx_j * FEAT_SIZE);
398 Sij = Hxs[i] * Px * Hxs[j].transpose() +
399 Hys[i] * Pxyi_t * Hxs[j].transpose() +
400 Hxs[i] * Pxyj * Hys[j].transpose() +
401 Hys[i] * Pyiyj * Hys[j].transpose();
407 S, OBS_SIZE * j, OBS_SIZE * i) = Sij.transpose();
411 const size_t obs_idx_off = i * OBS_SIZE;
412 Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(
413 S, obs_idx_off, obs_idx_off) +=
R;
421 S = Hxs[0] * m_pkk * Hxs[0].transpose() +
R;
424 m_timLogger.leave(
"KF:6.build S");
428 m_timLogger.enter(
"KF:7.get obs & DA");
431 OnGetObservationsAndDataAssociation(
433 all_predictions, S, predictLMidxs,
R 436 data_association.size() == Z.size() ||
437 (data_association.empty() && FEAT_SIZE == 0));
444 missing_predictions_to_add.clear();
447 for (
size_t i = 0; i < data_association.size(); ++i)
449 if (data_association[i] < 0)
continue;
450 const size_t assoc_idx_in_map =
451 static_cast<size_t>(data_association[i]);
452 const size_t assoc_idx_in_pred =
454 assoc_idx_in_map, 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 dh_dx_full_obs.zeros(
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 size_t 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, 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!");
564 dh_dx_full_obs, S_idxs.size(), 0) =
565 Hxs[assoc_idx_in_pred];
569 dh_dx_full_obs, S_idxs.size(),
570 VEH_SIZE + assoc_idx_in_map * FEAT_SIZE) =
571 Hys[assoc_idx_in_pred];
575 for (
size_t k = 0; k < OBS_SIZE; k++)
577 assoc_idx_in_pred * OBS_SIZE + k);
581 OnSubstractObservationVectors(
584 [predictLMidxs[assoc_idx_in_pred]]);
585 for (
size_t k = 0; k < OBS_SIZE; k++)
586 ytilde[ytilde_idx++] = ytilde_i[k];
590 S.extractSubmatrixSymmetrical(S_idxs, S_observed);
595 Z.size() == 1 && all_predictions.size() == 1);
597 dh_dx_full_obs.rows() == OBS_SIZE &&
598 dh_dx_full_obs.cols() == VEH_SIZE);
600 dh_dx_full_obs = Hxs[0];
602 OnSubstractObservationVectors(
603 ytilde_i, all_predictions[0]);
604 for (
size_t k = 0; k < OBS_SIZE; k++)
605 ytilde[ytilde_idx++] = ytilde_i[k];
613 m_timLogger.enter(
"KF:8.update stage:1.FULLKF:build K");
615 K.setSize(m_pkk.rows(), S_observed.cols());
618 K.multiply_ABt(m_pkk, dh_dx_full_obs);
624 m_timLogger.leave(
"KF:8.update stage:1.FULLKF:build K");
627 if (nKF_iterations == 1)
630 "KF:8.update stage:2.FULLKF:update xkk");
633 "KF:8.update stage:2.FULLKF:update xkk");
638 "KF:8.update stage:2.FULLKF:iter.update xkk");
641 dh_dx_full_obs.multiply_Ab(
642 m_xkk - xkk_0, HAx_column);
646 (ytilde - HAx_column), m_xkk,
651 "KF:8.update stage:2.FULLKF:iter.update xkk");
657 if (IKF_iteration == (nKF_iterations - 1))
660 "KF:8.update stage:3.FULLKF:update Pkk");
667 aux_K_dh_dx.multiply(K, dh_dx_full_obs);
670 const size_t stat_len = aux_K_dh_dx.cols();
671 for (
size_t r = 0;
r < stat_len;
r++)
673 for (
size_t c = 0;
c < stat_len;
c++)
676 aux_K_dh_dx.get_unsafe(
r,
c) =
677 -aux_K_dh_dx.get_unsafe(
r,
c) +
680 aux_K_dh_dx.get_unsafe(
r,
c) =
681 -aux_K_dh_dx.get_unsafe(
r,
c);
685 m_pkk.multiply_result_is_symmetric(
689 "KF:8.update stage:3.FULLKF:update Pkk");
702 for (
size_t obsIdx = 0; obsIdx < Z.size(); obsIdx++)
706 size_t idxInTheFilter = 0;
708 if (data_association.empty())
714 doit = data_association[obsIdx] >= 0;
715 if (doit) idxInTheFilter = data_association[obsIdx];
721 "KF:8.update stage:1.ScalarAtOnce.prepare");
724 const size_t idx_off =
733 std::vector<size_t>(1, idxInTheFilter), pred_obs);
738 OnSubstractObservationVectors(ytilde, pred_obs[0]);
745 const size_t i_idx_in_preds =
747 idxInTheFilter, predictLMidxs);
749 i_idx_in_preds != string::npos,
750 "OnPreComputingPredictions() didn't recommend the " 751 "prediction of a landmark which has been actually " 758 "KF:8.update stage:1.ScalarAtOnce.prepare");
761 for (
size_t j = 0; j < OBS_SIZE; j++)
764 "KF:8.update stage:2.ScalarAtOnce.update");
788 for (
size_t a = 0;
a < OBS_SIZE;
a++)
789 for (
size_t b = 0;
b < OBS_SIZE;
b++)
793 "This KF algorithm assumes " 796 "observation (matrix R). " 802 KFTYPE Sij =
R.get_unsafe(j, j);
805 for (
size_t k = 0; k < VEH_SIZE; k++)
808 for (
size_t q = 0;
q < VEH_SIZE;
q++)
809 accum += Hx.get_unsafe(j,
q) *
810 m_pkk.get_unsafe(
q, k);
811 Sij += Hx.get_unsafe(j, k) * accum;
816 for (
size_t k = 0; k < VEH_SIZE; k++)
819 for (
size_t q = 0;
q < FEAT_SIZE;
q++)
820 accum += Hy.get_unsafe(j,
q) *
821 m_pkk.get_unsafe(idx_off +
q, k);
822 term2 += Hx.get_unsafe(j, k) * accum;
827 for (
size_t k = 0; k < FEAT_SIZE; k++)
830 for (
size_t q = 0;
q < FEAT_SIZE;
q++)
831 accum += Hy.get_unsafe(j,
q) *
833 idx_off +
q, idx_off + k);
834 Sij += Hy.get_unsafe(j, k) * accum;
840 size_t N = m_pkk.cols();
841 vector<KFTYPE> Kij(N);
843 for (
size_t k = 0; k < N; k++)
849 for (
q = 0;
q < VEH_SIZE;
q++)
850 K_tmp += m_pkk.get_unsafe(k,
q) *
854 for (
q = 0;
q < FEAT_SIZE;
q++)
855 K_tmp += m_pkk.get_unsafe(k, idx_off +
q) *
858 Kij[k] = K_tmp / Sij;
863 for (
size_t k = 0; k < N; k++)
864 m_xkk[k] += Kij[k] * ytilde[j];
869 for (
size_t k = 0; k < N; k++)
871 for (
size_t q = k;
q < N;
874 m_pkk(k,
q) -= Sij * Kij[k] * Kij[
q];
876 m_pkk(
q, k) = m_pkk(k,
q);
879 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 882 m_pkk.saveToTextFile(
"Pkk_err.txt");
892 "KF:8.update stage:2.ScalarAtOnce.update");
909 size_t nKF_iterations = KF_options.IKF_iterations;
913 if (nKF_iterations>1)
923 for (
size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
931 xkk_next_iter = xkk_0;
935 for (
size_t obsIdx=0;obsIdx<Z.rows();obsIdx++)
939 size_t idxInTheFilter=0;
941 if (data_association.empty())
947 doit = data_association[obsIdx] >= 0;
949 idxInTheFilter = data_association[obsIdx];
955 const size_t idx_off = VEH_SIZE + idxInTheFilter *FEAT_SIZE;
956 const size_t R_row_offset = obsIdx*OBS_SIZE;
960 OnObservationModelAndJacobians(
1000 R.extractMatrix(R_row_offset,0, Si);
1016 KFMatrix Pyix( FEAT_SIZE, VEH_SIZE );
1017 m_pkk.extractMatrix(idx_off,0, Pyix);
1019 term.multiply_ABCt( Hy, Pyix, Hx );
1036 size_t N = m_pkk.cols();
1044 for (
size_t c=0;
c<OBS_SIZE;
c++)
1049 for (
q=0;
q<VEH_SIZE;
q++)
1050 K_tmp+= m_pkk.get_unsafe(k,
q) * Hx.get_unsafe(
c,
q);
1053 for (
q=0;
q<FEAT_SIZE;
q++)
1054 K_tmp+= m_pkk.get_unsafe(k,idx_off+
q) * Hy.get_unsafe(
c,
q);
1056 Ki.set_unsafe(k,
c, K_tmp);
1060 Ki.multiply(Ki, Si.inv() );
1064 if (nKF_iterations==1)
1069 for (
size_t q=0;
q<OBS_SIZE;
q++)
1070 m_xkk[k] += Ki.get_unsafe(k,
q) * ytilde[
q];
1075 Eigen::Matrix<KFTYPE,Eigen::Dynamic,1> HAx(OBS_SIZE);
1078 for (o=0;o<OBS_SIZE;o++)
1081 for (
q=0;
q<VEH_SIZE;
q++)
1082 tmp += Hx.get_unsafe(o,
q) * (xkk_0[
q] - m_xkk[
q]);
1084 for (
q=0;
q<FEAT_SIZE;
q++)
1085 tmp += Hy.get_unsafe(o,
q) * (xkk_0[idx_off+
q] - m_xkk[idx_off+
q]);
1091 for (o=0;o<OBS_SIZE;o++)
1092 HAx[o] = ytilde[o] - HAx[o];
1098 KFTYPE tmp = xkk_next_iter[k];
1100 for (o=0;o<OBS_SIZE;o++)
1101 tmp += Ki.get_unsafe(k,o) * HAx[o];
1103 xkk_next_iter[k] = tmp;
1112 Ki.multiplyByMatrixAndByTransposeNonSymmetric(
1118 m_pkk.force_symmetry();
1146 if (nKF_iterations>1)
1149 cout <<
"IKF iter: " << IKF_iteration <<
" -> " << xkk_next_iter << endl;
1151 m_xkk = xkk_next_iter;
1157 if (saved_Pkk)
delete saved_Pkk;
1168 const double tim_update = m_timLogger.leave(
"KF:8.update stage");
1170 m_timLogger.enter(
"KF:9.OnNormalizeStateVector");
1171 OnNormalizeStateVector();
1172 m_timLogger.leave(
"KF:9.OnNormalizeStateVector");
1177 if (!data_association.empty())
1179 m_timLogger.enter(
"KF:A.add new landmarks");
1181 m_timLogger.leave(
"KF:A.add new landmarks");
1185 m_timLogger.enter(
"KF:B.OnPostIteration");
1187 m_timLogger.leave(
"KF:B.OnPostIteration");
1189 m_timLogger.leave(
"KF:complete_step");
1192 "[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: " 1194 static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
1195 1e3 * tim_pred, 1e3 * tim_pred_obs, 1e3 * tim_obs_DA,
1201 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1205 const KFArray_VEH&
x,
const std::pair<KFCLASS*, KFArray_ACT>& dat,
1210 dat.first->OnTransitionModel(dat.second, out_x, dumm);
1214 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1218 const KFArray_VEH&
x,
const std::pair<KFCLASS*, size_t>& dat,
1221 std::vector<size_t> idxs_to_predict(1, dat.second);
1224 ::memcpy(&dat.first->m_xkk[0], &
x[0],
sizeof(
x[0]) * VEH_SIZE);
1225 dat.first->OnObservationModel(idxs_to_predict, prediction);
1227 out_x = prediction[0];
1231 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1235 const KFArray_FEAT&
x,
const std::pair<KFCLASS*, size_t>& dat,
1238 std::vector<size_t> idxs_to_predict(1, dat.second);
1240 const size_t lm_idx_in_statevector = VEH_SIZE + FEAT_SIZE * dat.second;
1243 &dat.first->m_xkk[lm_idx_in_statevector], &
x[0],
1244 sizeof(
x[0]) * FEAT_SIZE);
1245 dat.first->OnObservationModel(idxs_to_predict, prediction);
1247 out_x = prediction[0];
1254 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1259 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
1260 const std::vector<int>& data_association,
1262 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::KFMatrix_OxO&
R)
1267 for (
size_t idxObs = 0; idxObs < Z.size(); idxObs++)
1270 if (data_association[idxObs] < 0)
1278 0 == ((
obj.internal_getXkk().size() - VEH_SIZE) %
1281 const size_t newIndexInMap =
1282 (
obj.internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
1285 typename KF::KFArray_FEAT yn;
1286 typename KF::KFMatrix_FxV dyn_dxv;
1287 typename KF::KFMatrix_FxO dyn_dhn;
1288 typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1289 bool use_dyn_dhn_jacobian =
true;
1292 obj.OnInverseObservationModel(
1293 Z[idxObs], yn, dyn_dxv, dyn_dhn, dyn_dhn_R_dyn_dhnT,
1294 use_dyn_dhn_jacobian);
1298 obj.OnNewLandmarkAddedToMap(idxObs, newIndexInMap);
1304 size_t idx =
obj.internal_getXkk().size();
1305 obj.internal_getXkk().conservativeResize(
1306 obj.internal_getXkk().size() + FEAT_SIZE);
1308 for (
q = 0;
q < FEAT_SIZE;
q++)
1309 obj.internal_getXkk()[idx +
q] = yn[
q];
1315 obj.internal_getPkk().cols() == (int)idx &&
1316 obj.internal_getPkk().rows() == (int)idx);
1318 obj.internal_getPkk().setSize(idx + FEAT_SIZE, idx + FEAT_SIZE);
1322 typename KF::KFMatrix_VxV Pxx;
1323 obj.internal_getPkk().extractMatrix(0, 0, Pxx);
1324 typename KF::KFMatrix_FxV Pxyn;
1325 Pxyn.multiply(dyn_dxv, Pxx);
1327 obj.internal_getPkk().insertMatrix(idx, 0, Pxyn);
1328 obj.internal_getPkk().insertMatrixTranspose(0, idx, Pxyn);
1333 (idx - VEH_SIZE) / FEAT_SIZE;
1334 for (
q = 0;
q < nLMs;
q++)
1336 typename KF::KFMatrix_VxF P_x_yq(
1338 obj.internal_getPkk().extractMatrix(
1339 0, VEH_SIZE +
q * FEAT_SIZE, P_x_yq);
1341 typename KF::KFMatrix_FxF P_cross(
1343 P_cross.multiply(dyn_dxv, P_x_yq);
1345 obj.internal_getPkk().insertMatrix(
1346 idx, VEH_SIZE +
q * FEAT_SIZE, P_cross);
1347 obj.internal_getPkk().insertMatrixTranspose(
1348 VEH_SIZE +
q * FEAT_SIZE, idx, P_cross);
1356 dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
1357 if (use_dyn_dhn_jacobian)
1358 dyn_dhn.multiply_HCHt(
1361 P_yn_yn += dyn_dhn_R_dyn_dhnT;
1363 obj.internal_getPkk().insertMatrix(idx, idx, P_yn_yn);
1365 obj.getProfiler().leave(
"KF:9.create new LMs");
1370 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1373 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>&
obj,
1375 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
1376 KFTYPE>::vector_KFArray_OBS& Z,
1377 const std::vector<int>& data_association,
1379 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
1380 KFTYPE>::KFMatrix_OxO&
R)
1386 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1392 return (
obj.getStateVectorLength() - VEH_SIZE) / FEAT_SIZE;
1395 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1398 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>&
obj)
1404 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1410 return (
obj.getStateVectorLength() == VEH_SIZE);
1413 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1416 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>&
obj)
Eigen::Matrix< double, Eigen::Dynamic, 1 > KFVector
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
#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...
GLsizei GLsizei GLuint * obj
#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.
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...
#define ASSERT_(f)
Defines an assertion mechanism.
A numeric matrix of compile-time fixed size.
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)
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 ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
double kftype
The numeric type used in the Kalman Filter (default=double)
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::AutoAlign|Eigen::RowMajor > Base
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
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.
GLubyte GLubyte GLubyte a
mrpt::system::CTimeLogger & getProfiler()
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
void enter(const char *func_name)
Start of a named section.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".