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,
23 void CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE,
24 KFTYPE>::runOneKalmanIteration()
29 m_timLogger.enable(KF_options.enable_profiler);
30 m_timLogger.enter(
"KF:complete_step");
32 ASSERT_(
size_t(m_xkk.size()) == m_pkk.getColCount())
33 ASSERT_(
size_t(m_xkk.size()) >= VEH_SIZE)
40 m_timLogger.enter(
"KF:1.OnGetAction");
42 m_timLogger.leave(
"KF:1.OnGetAction");
48 (((m_xkk.size() - VEH_SIZE) / FEAT_SIZE) * FEAT_SIZE) ==
49 (m_xkk.size() - VEH_SIZE))
55 m_timLogger.enter(
"KF:2.prediction stage");
57 const size_t N_map = getNumberOfLandmarksInTheMap();
61 bool skipPrediction =
false;
69 OnTransitionModel(u, xv, skipPrediction);
81 m_user_didnt_implement_jacobian =
false;
84 if (KF_options.use_analytic_transition_jacobian)
85 OnTransitionJacobian(dfv_dxv);
87 if (m_user_didnt_implement_jacobian ||
88 !KF_options.use_analytic_transition_jacobian ||
89 KF_options.debug_verify_analytic_jacobians)
94 OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
100 const std::pair<KFCLASS*, KFArray_ACT>& dat,
101 KFArray_VEH& out_x)>(&KF_aux_estimate_trans_jacobian),
102 xkk_veh_increments, std::pair<KFCLASS*, KFArray_ACT>(
this, u),
105 if (KF_options.debug_verify_analytic_jacobians)
108 OnTransitionJacobian(dfv_dxv_gt);
109 if ((dfv_dxv - dfv_dxv_gt).array().abs().
sum() >
110 KF_options.debug_verify_analytic_jacobians_threshold)
112 std::cerr <<
"[KalmanFilter] ERROR: User analytical " 113 "transition Jacobians are wrong: \n" 114 <<
" Real dfv_dxv: \n" 115 << dfv_dxv <<
"\n Analytical dfv_dxv:\n" 116 << dfv_dxv_gt <<
"Diff:\n" 117 << (dfv_dxv - dfv_dxv_gt) <<
"\n";
119 "ERROR: User analytical transition Jacobians are wrong " 120 "(More details dumped to cerr)")
128 OnTransitionNoise(Q);
134 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, VEH_SIZE>(m_pkk, 0, 0) =
136 dfv_dxv * Eigen::Block<typename KFMatrix::Base, VEH_SIZE, VEH_SIZE>(
145 for (
size_t i = 0; i < N_map; i++)
148 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>(
149 m_pkk, 0, VEH_SIZE + i * FEAT_SIZE);
151 Eigen::Block<typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>(
152 m_pkk, 0, VEH_SIZE + i * FEAT_SIZE) = aux;
153 Eigen::Block<typename KFMatrix::Base, FEAT_SIZE, VEH_SIZE>(
154 m_pkk, VEH_SIZE + i * FEAT_SIZE, 0) = aux.transpose();
160 for (
size_t i = 0; i < VEH_SIZE; i++) m_xkk[i] = xv[i];
163 OnNormalizeStateVector();
167 const double tim_pred = m_timLogger.leave(
"KF:2.prediction stage");
172 m_timLogger.enter(
"KF:3.predict all obs");
175 OnGetObservationNoise(
R);
179 all_predictions.resize(N_map);
181 mrpt::math::sequenceStdVec<size_t, 1>(0, N_map), all_predictions);
183 const double tim_pred_obs = m_timLogger.leave(
"KF:3.predict all obs");
185 m_timLogger.enter(
"KF:4.decide pred obs");
188 predictLMidxs.clear();
192 predictLMidxs.assign(1, 0);
195 OnPreComputingPredictions(all_predictions, predictLMidxs);
197 m_timLogger.leave(
"KF:4.decide pred obs");
220 m_timLogger.enter(
"KF:5.build Jacobians");
225 : predictLMidxs.size();
234 std::vector<size_t> missing_predictions_to_add;
239 size_t first_new_pred = 0;
245 if (!missing_predictions_to_add.empty())
247 const size_t nNew = missing_predictions_to_add.size();
249 "[KF] *Performance Warning*: " 250 << nNew <<
" LMs were not correctly predicted by " 251 "OnPreComputingPredictions().");
254 for (
size_t j = 0; j < nNew; j++)
255 predictLMidxs.push_back(missing_predictions_to_add[j]);
257 N_pred = predictLMidxs.size();
258 missing_predictions_to_add.clear();
264 for (
size_t i = first_new_pred; i < N_pred; ++i)
266 const size_t lm_idx = FEAT_SIZE == 0 ? 0 : predictLMidxs[i];
271 m_user_didnt_implement_jacobian =
274 if (KF_options.use_analytic_observation_jacobian)
275 OnObservationJacobians(lm_idx, Hx, Hy);
277 if (m_user_didnt_implement_jacobian ||
278 !KF_options.use_analytic_observation_jacobian ||
279 KF_options.debug_verify_analytic_jacobians)
281 const size_t lm_idx_in_statevector =
282 VEH_SIZE + lm_idx * FEAT_SIZE;
285 const KFArray_FEAT x_feat(&m_xkk[lm_idx_in_statevector]);
289 OnObservationJacobiansNumericGetIncrements(
290 xkk_veh_increments, feat_increments);
296 const std::pair<KFCLASS*, size_t>& dat,
297 KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hx_jacobian),
299 std::pair<KFCLASS*, size_t>(
this, lm_idx), Hx);
302 ::memcpy(&m_xkk[0], &x_vehicle[0],
sizeof(m_xkk[0]) * VEH_SIZE);
308 const std::pair<KFCLASS*, size_t>& dat,
309 KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hy_jacobian),
310 feat_increments, std::pair<KFCLASS*, size_t>(
this, lm_idx),
315 &m_xkk[lm_idx_in_statevector], &x_feat[0],
316 sizeof(m_xkk[0]) * FEAT_SIZE);
318 if (KF_options.debug_verify_analytic_jacobians)
322 OnObservationJacobians(lm_idx, Hx_gt, Hy_gt);
323 if ((Hx - Hx_gt).array().abs().
sum() >
324 KF_options.debug_verify_analytic_jacobians_threshold)
326 std::cerr <<
"[KalmanFilter] ERROR: User analytical " 327 "observation Hx Jacobians are wrong: \n" 329 << Hx <<
"\n Analytical Hx:\n" 330 << Hx_gt <<
"Diff:\n" 331 << Hx - Hx_gt <<
"\n";
333 "ERROR: User analytical observation Hx Jacobians " 334 "are wrong (More details dumped to cerr)")
336 if ((Hy - Hy_gt).array().abs().
sum() >
337 KF_options.debug_verify_analytic_jacobians_threshold)
339 std::cerr <<
"[KalmanFilter] ERROR: User analytical " 340 "observation Hy Jacobians are wrong: \n" 342 << Hy <<
"\n Analytical Hx:\n" 343 << Hy_gt <<
"Diff:\n" 344 << Hy - Hy_gt <<
"\n";
346 "ERROR: User analytical observation Hy Jacobians " 347 "are wrong (More details dumped to cerr)")
352 m_timLogger.leave(
"KF:5.build Jacobians");
354 m_timLogger.enter(
"KF:6.build S");
361 S.setSize(N_pred * OBS_SIZE, N_pred * OBS_SIZE);
369 for (
size_t i = 0; i < N_pred; ++i)
371 const size_t lm_idx_i = predictLMidxs[i];
375 m_pkk, VEH_SIZE + lm_idx_i * FEAT_SIZE, 0);
378 for (
size_t j = i; j < N_pred; ++j)
380 const size_t lm_idx_j = predictLMidxs[j];
382 Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>
383 Sij(S, OBS_SIZE * i, OBS_SIZE * j);
387 Pxyj(m_pkk, 0, VEH_SIZE + lm_idx_j * FEAT_SIZE);
391 m_pkk, VEH_SIZE + lm_idx_i * FEAT_SIZE,
392 VEH_SIZE + lm_idx_j * FEAT_SIZE);
394 Sij = Hxs[i] * Px * Hxs[j].transpose() +
395 Hys[i] * Pxyi_t * Hxs[j].transpose() +
396 Hxs[i] * Pxyj * Hys[j].transpose() +
397 Hys[i] * Pyiyj * Hys[j].transpose();
402 OBS_SIZE>(S, OBS_SIZE * j, OBS_SIZE * i) =
407 const size_t obs_idx_off = i * OBS_SIZE;
408 Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(
409 S, obs_idx_off, obs_idx_off) +=
R;
420 m_timLogger.leave(
"KF:6.build S");
424 m_timLogger.enter(
"KF:7.get obs & DA");
427 OnGetObservationsAndDataAssociation(
429 all_predictions, S, predictLMidxs,
R 432 data_association.size() == Z.size() ||
433 (data_association.empty() && FEAT_SIZE == 0));
440 missing_predictions_to_add.clear();
443 for (
size_t i = 0; i < data_association.size(); ++i)
445 if (data_association[i] < 0)
continue;
446 const size_t assoc_idx_in_map =
447 static_cast<size_t>(data_association[i]);
449 assoc_idx_in_map, predictLMidxs);
450 if (assoc_idx_in_pred == std::string::npos)
451 missing_predictions_to_add.push_back(assoc_idx_in_map);
455 first_new_pred = N_pred;
458 }
while (!missing_predictions_to_add.empty());
460 const double tim_obs_DA = m_timLogger.leave(
"KF:7.get obs & DA");
468 m_timLogger.enter(
"KF:8.update stage");
470 switch (KF_options.method)
481 vector_int mapIndicesForKFUpdate(data_association.size());
482 mapIndicesForKFUpdate.resize(
484 mapIndicesForKFUpdate.begin(),
486 data_association.begin(), data_association.end(),
487 mapIndicesForKFUpdate.begin(),
488 bind1st(equal_to<int>(), -1))));
491 (FEAT_SIZE == 0) ? 1 :
494 mapIndicesForKFUpdate
498 const size_t nKF_iterations = (KF_options.method ==
kfEKFNaive)
500 : KF_options.IKF_iterations;
507 for (
size_t IKF_iteration = 0;
508 IKF_iteration < nKF_iterations; IKF_iteration++)
512 size_t ytilde_idx = 0;
516 dh_dx_full_obs.zeros(
518 VEH_SIZE + FEAT_SIZE * N_map);
526 S_idxs.reserve(OBS_SIZE * N_upd);
531 for (
size_t i = 0; i < data_association.size(); ++i)
533 if (data_association[i] < 0)
continue;
535 const size_t assoc_idx_in_map =
536 static_cast<size_t>(data_association[i]);
537 const size_t assoc_idx_in_pred =
539 assoc_idx_in_map, predictLMidxs);
541 assoc_idx_in_pred != string::npos,
542 "OnPreComputingPredictions() didn't " 543 "recommend the prediction of a landmark " 544 "which has been actually observed!")
559 dh_dx_full_obs, S_idxs.size(), 0) =
560 Hxs[assoc_idx_in_pred];
563 dh_dx_full_obs, S_idxs.size(),
564 VEH_SIZE + assoc_idx_in_map * FEAT_SIZE) =
565 Hys[assoc_idx_in_pred];
569 for (
size_t k = 0; k < OBS_SIZE; k++)
571 assoc_idx_in_pred * OBS_SIZE + k);
575 OnSubstractObservationVectors(
578 [predictLMidxs[assoc_idx_in_pred]]);
579 for (
size_t k = 0; k < OBS_SIZE; k++)
580 ytilde[ytilde_idx++] = ytilde_i[k];
584 S.extractSubmatrixSymmetrical(S_idxs, S_observed);
589 Z.size() == 1 && all_predictions.size() == 1)
591 dh_dx_full_obs.getRowCount() == OBS_SIZE &&
592 dh_dx_full_obs.getColCount() == VEH_SIZE)
595 dh_dx_full_obs = Hxs[0];
597 OnSubstractObservationVectors(
598 ytilde_i, all_predictions[0]);
599 for (
size_t k = 0; k < OBS_SIZE; k++)
600 ytilde[ytilde_idx++] = ytilde_i[k];
608 m_timLogger.enter(
"KF:8.update stage:1.FULLKF:build K");
611 m_pkk.getRowCount(), S_observed.getColCount());
614 K.multiply_ABt(m_pkk, dh_dx_full_obs);
620 m_timLogger.leave(
"KF:8.update stage:1.FULLKF:build K");
623 if (nKF_iterations == 1)
626 "KF:8.update stage:2.FULLKF:update xkk");
629 "KF:8.update stage:2.FULLKF:update xkk");
634 "KF:8.update stage:2.FULLKF:iter.update xkk");
637 dh_dx_full_obs.multiply_Ab(
638 m_xkk - xkk_0, HAx_column);
642 (ytilde - HAx_column), m_xkk,
647 "KF:8.update stage:2.FULLKF:iter.update xkk");
653 if (IKF_iteration == (nKF_iterations - 1))
656 "KF:8.update stage:3.FULLKF:update Pkk");
663 aux_K_dh_dx.multiply(K, dh_dx_full_obs);
666 const size_t stat_len = aux_K_dh_dx.getColCount();
667 for (
size_t r = 0;
r < stat_len;
r++)
669 for (
size_t c = 0;
c < stat_len;
c++)
672 aux_K_dh_dx.get_unsafe(
r,
c) =
673 -aux_K_dh_dx.get_unsafe(
r,
c) +
676 aux_K_dh_dx.get_unsafe(
r,
c) =
677 -aux_K_dh_dx.get_unsafe(
r,
c);
681 m_pkk.multiply_result_is_symmetric(
685 "KF:8.update stage:3.FULLKF:update Pkk");
698 for (
size_t obsIdx = 0; obsIdx < Z.size(); obsIdx++)
702 size_t idxInTheFilter = 0;
704 if (data_association.empty())
710 doit = data_association[obsIdx] >= 0;
711 if (doit) idxInTheFilter = data_association[obsIdx];
717 "KF:8.update stage:1.ScalarAtOnce.prepare");
720 const size_t idx_off =
734 OnSubstractObservationVectors(ytilde, pred_obs[0]);
741 const size_t i_idx_in_preds =
743 idxInTheFilter, predictLMidxs);
745 i_idx_in_preds != string::npos,
746 "OnPreComputingPredictions() didn't recommend the " 747 "prediction of a landmark which has been actually " 754 "KF:8.update stage:1.ScalarAtOnce.prepare");
757 for (
size_t j = 0; j < OBS_SIZE; j++)
760 "KF:8.update stage:2.ScalarAtOnce.update");
781 for (
size_t a = 0;
a < OBS_SIZE;
a++)
782 for (
size_t b = 0;
b < OBS_SIZE;
b++)
786 "This KF algorithm assumes " 789 "observation (matrix R). " 795 KFTYPE Sij =
R.get_unsafe(j, j);
798 for (
size_t k = 0; k < VEH_SIZE; k++)
801 for (
size_t q = 0;
q < VEH_SIZE;
q++)
802 accum += Hx.get_unsafe(j,
q) *
803 m_pkk.get_unsafe(
q, k);
804 Sij += Hx.get_unsafe(j, k) * accum;
809 for (
size_t k = 0; k < VEH_SIZE; k++)
812 for (
size_t q = 0;
q < FEAT_SIZE;
q++)
813 accum += Hy.get_unsafe(j,
q) *
814 m_pkk.get_unsafe(idx_off +
q, k);
815 term2 += Hx.get_unsafe(j, k) * accum;
820 for (
size_t k = 0; k < FEAT_SIZE; k++)
823 for (
size_t q = 0;
q < FEAT_SIZE;
q++)
824 accum += Hy.get_unsafe(j,
q) *
826 idx_off +
q, idx_off + k);
827 Sij += Hy.get_unsafe(j, k) * accum;
833 size_t N = m_pkk.getColCount();
834 vector<KFTYPE> Kij(N);
836 for (
size_t k = 0; k < N; k++)
842 for (
q = 0;
q < VEH_SIZE;
q++)
843 K_tmp += m_pkk.get_unsafe(k,
q) *
847 for (
q = 0;
q < FEAT_SIZE;
q++)
848 K_tmp += m_pkk.get_unsafe(k, idx_off +
q) *
851 Kij[k] = K_tmp / Sij;
856 for (
size_t k = 0; k < N; k++)
857 m_xkk[k] += Kij[k] * ytilde[j];
862 for (
size_t k = 0; k < N; k++)
864 for (
size_t q = k;
q < N;
867 m_pkk(k,
q) -= Sij * Kij[k] * Kij[
q];
869 m_pkk(
q, k) = m_pkk(k,
q);
872 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 875 m_pkk.saveToTextFile(
"Pkk_err.txt");
885 "KF:8.update stage:2.ScalarAtOnce.update");
902 size_t nKF_iterations = KF_options.IKF_iterations;
906 if (nKF_iterations>1)
916 for (
size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
924 xkk_next_iter = xkk_0;
928 for (
size_t obsIdx=0;obsIdx<Z.getRowCount();obsIdx++)
932 size_t idxInTheFilter=0;
934 if (data_association.empty())
940 doit = data_association[obsIdx] >= 0;
942 idxInTheFilter = data_association[obsIdx];
948 const size_t idx_off = VEH_SIZE + idxInTheFilter *FEAT_SIZE;
949 const size_t R_row_offset = obsIdx*OBS_SIZE;
953 OnObservationModelAndJacobians(
993 R.extractMatrix(R_row_offset,0, Si);
1009 KFMatrix Pyix( FEAT_SIZE, VEH_SIZE );
1010 m_pkk.extractMatrix(idx_off,0, Pyix);
1012 term.multiply_ABCt( Hy, Pyix, Hx );
1029 size_t N = m_pkk.getColCount();
1037 for (
size_t c=0;
c<OBS_SIZE;
c++)
1042 for (
q=0;
q<VEH_SIZE;
q++)
1043 K_tmp+= m_pkk.get_unsafe(k,
q) * Hx.get_unsafe(
c,
q);
1046 for (
q=0;
q<FEAT_SIZE;
q++)
1047 K_tmp+= m_pkk.get_unsafe(k,idx_off+
q) * Hy.get_unsafe(
c,
q);
1049 Ki.set_unsafe(k,
c, K_tmp);
1053 Ki.multiply(Ki, Si.inv() );
1057 if (nKF_iterations==1)
1062 for (
size_t q=0;
q<OBS_SIZE;
q++)
1063 m_xkk[k] += Ki.get_unsafe(k,
q) * ytilde[
q];
1068 Eigen::Matrix<KFTYPE,Eigen::Dynamic,1> HAx(OBS_SIZE);
1071 for (o=0;o<OBS_SIZE;o++)
1074 for (
q=0;
q<VEH_SIZE;
q++)
1075 tmp += Hx.get_unsafe(o,
q) * (xkk_0[
q] - m_xkk[
q]);
1077 for (
q=0;
q<FEAT_SIZE;
q++)
1078 tmp += Hy.get_unsafe(o,
q) * (xkk_0[idx_off+
q] - m_xkk[idx_off+
q]);
1084 for (o=0;o<OBS_SIZE;o++)
1085 HAx[o] = ytilde[o] - HAx[o];
1091 KFTYPE tmp = xkk_next_iter[k];
1093 for (o=0;o<OBS_SIZE;o++)
1094 tmp += Ki.get_unsafe(k,o) * HAx[o];
1096 xkk_next_iter[k] = tmp;
1105 Ki.multiplyByMatrixAndByTransposeNonSymmetric(
1111 m_pkk.force_symmetry();
1139 if (nKF_iterations>1)
1142 cout <<
"IKF iter: " << IKF_iteration <<
" -> " << xkk_next_iter << endl;
1144 m_xkk = xkk_next_iter;
1150 if (saved_Pkk)
delete saved_Pkk;
1161 const double tim_update = m_timLogger.leave(
"KF:8.update stage");
1163 m_timLogger.enter(
"KF:9.OnNormalizeStateVector");
1164 OnNormalizeStateVector();
1165 m_timLogger.leave(
"KF:9.OnNormalizeStateVector");
1170 if (!data_association.empty())
1172 m_timLogger.enter(
"KF:A.add new landmarks");
1174 m_timLogger.leave(
"KF:A.add new landmarks");
1178 m_timLogger.enter(
"KF:B.OnPostIteration");
1180 m_timLogger.leave(
"KF:B.OnPostIteration");
1182 m_timLogger.leave(
"KF:complete_step");
1186 "[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: " 1188 static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
1189 1e3 * tim_pred, 1e3 * tim_pred_obs, 1e3 * tim_obs_DA,
1194 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1198 const KFArray_VEH&
x,
const std::pair<KFCLASS*, KFArray_ACT>& dat,
1203 dat.first->OnTransitionModel(dat.second, out_x, dumm);
1206 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1210 const KFArray_VEH&
x,
const std::pair<KFCLASS*, size_t>& dat,
1216 ::memcpy(&dat.first->m_xkk[0], &
x[0],
sizeof(
x[0]) * VEH_SIZE);
1217 dat.first->OnObservationModel(idxs_to_predict, prediction);
1219 out_x = prediction[0];
1222 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1226 const KFArray_FEAT&
x,
const std::pair<KFCLASS*, size_t>& dat,
1231 const size_t lm_idx_in_statevector = VEH_SIZE + FEAT_SIZE * dat.second;
1234 &dat.first->m_xkk[lm_idx_in_statevector], &
x[0],
1235 sizeof(
x[0]) * FEAT_SIZE);
1236 dat.first->OnObservationModel(idxs_to_predict, prediction);
1238 out_x = prediction[0];
1244 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1249 KFTYPE>::vector_KFArray_OBS& Z,
1252 KFTYPE>::KFMatrix_OxO&
R)
1258 for (
size_t idxObs = 0; idxObs < Z.size(); idxObs++)
1261 if (data_association[idxObs] < 0)
1269 0 == ((
obj.internal_getXkk().size() - VEH_SIZE) %
1272 const size_t newIndexInMap =
1273 (
obj.internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
1276 typename KF::KFArray_FEAT yn;
1277 typename KF::KFMatrix_FxV dyn_dxv;
1278 typename KF::KFMatrix_FxO dyn_dhn;
1279 typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1280 bool use_dyn_dhn_jacobian =
true;
1283 obj.OnInverseObservationModel(
1284 Z[idxObs], yn, dyn_dxv, dyn_dhn, dyn_dhn_R_dyn_dhnT,
1285 use_dyn_dhn_jacobian);
1289 obj.OnNewLandmarkAddedToMap(idxObs, newIndexInMap);
1295 size_t idx =
obj.internal_getXkk().size();
1296 obj.internal_getXkk().conservativeResize(
1297 obj.internal_getXkk().size() + FEAT_SIZE);
1299 for (
q = 0;
q < FEAT_SIZE;
q++)
1300 obj.internal_getXkk()[idx +
q] = yn[
q];
1306 obj.internal_getPkk().getColCount() == idx &&
1307 obj.internal_getPkk().getRowCount() == idx);
1309 obj.internal_getPkk().setSize(idx + FEAT_SIZE, idx + FEAT_SIZE);
1313 typename KF::KFMatrix_VxV Pxx;
1314 obj.internal_getPkk().extractMatrix(0, 0, Pxx);
1315 typename KF::KFMatrix_FxV Pxyn;
1316 Pxyn.multiply(dyn_dxv, Pxx);
1318 obj.internal_getPkk().insertMatrix(idx, 0, Pxyn);
1319 obj.internal_getPkk().insertMatrixTranspose(0, idx, Pxyn);
1324 (idx - VEH_SIZE) / FEAT_SIZE;
1325 for (
q = 0;
q < nLMs;
q++)
1327 typename KF::KFMatrix_VxF P_x_yq(
1329 obj.internal_getPkk().extractMatrix(
1330 0, VEH_SIZE +
q * FEAT_SIZE, P_x_yq);
1332 typename KF::KFMatrix_FxF P_cross(
1334 P_cross.multiply(dyn_dxv, P_x_yq);
1336 obj.internal_getPkk().insertMatrix(
1337 idx, VEH_SIZE +
q * FEAT_SIZE, P_cross);
1338 obj.internal_getPkk().insertMatrixTranspose(
1339 VEH_SIZE +
q * FEAT_SIZE, idx, P_cross);
1347 dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
1348 if (use_dyn_dhn_jacobian)
1349 dyn_dhn.multiply_HCHt(
1352 P_yn_yn += dyn_dhn_R_dyn_dhnT;
1354 obj.internal_getPkk().insertMatrix(idx, idx, P_yn_yn);
1356 obj.getProfiler().leave(
"KF:9.create new LMs");
1361 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1366 ACT_SIZE, KFTYPE>::vector_KFArray_OBS&
1370 ACT_SIZE, KFTYPE>::KFMatrix_OxO&
R)
1375 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1381 return (
obj.getStateVectorLength() - VEH_SIZE) / FEAT_SIZE;
1384 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
1392 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
1398 return (
obj.getStateVectorLength() == VEH_SIZE);
1401 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
mrpt::utils::CTimeLogger & getProfiler()
Eigen::Matrix< double, Eigen::Dynamic, 1 > KFVector
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::AutoAlign|Eigen::RowMajor > Base
#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
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...
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)
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)
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.
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 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".