00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CKalmanFilterCapable_H
00029 #define CKalmanFilterCapable_H
00030
00031 #include <mrpt/math/CMatrixFixedNumeric.h>
00032 #include <mrpt/math/CMatrixTemplateNumeric.h>
00033 #include <mrpt/math/CArray.h>
00034 #include <mrpt/math/utils.h>
00035
00036 #include <mrpt/utils/CTimeLogger.h>
00037 #include <mrpt/utils/CLoadableOptions.h>
00038 #include <mrpt/utils/CDebugOutputCapable.h>
00039 #include <mrpt/utils/stl_extensions.h>
00040 #include <mrpt/system/os.h>
00041 #include <mrpt/utils/CTicTac.h>
00042 #include <mrpt/utils/CFileOutputStream.h>
00043
00044 #include <mrpt/bayes/link_pragmas.h>
00045
00046
00047 namespace mrpt
00048 {
00049 namespace bayes
00050 {
00051 using namespace mrpt::utils;
00052 using namespace mrpt::math;
00053 using namespace mrpt;
00054 using namespace std;
00055
00056
00057
00058
00059
00060 enum TKFMethod {
00061 kfEKFNaive = 0,
00062 kfEKFAlaDavison,
00063 kfIKFFull,
00064 kfIKF
00065 };
00066
00067
00068 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE> class CKalmanFilterCapable;
00069
00070 namespace detail {
00071 struct CRunOneKalmanIteration_addNewLandmarks;
00072 }
00073
00074
00075
00076 struct BAYES_IMPEXP TKF_options : public utils::CLoadableOptions
00077 {
00078 TKF_options();
00079
00080 void loadFromConfigFile(
00081 const mrpt::utils::CConfigFileBase &source,
00082 const std::string §ion);
00083
00084
00085 void dumpToTextStream(CStream &out) const;
00086
00087 TKFMethod method;
00088 bool verbose;
00089 int IKF_iterations;
00090 bool enable_profiler;
00091 bool use_analytic_transition_jacobian;
00092 bool use_analytic_observation_jacobian;
00093 bool debug_verify_analytic_jacobians;
00094 double debug_verify_analytic_jacobians_threshold;
00095 };
00096
00097
00098 namespace detail
00099 {
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
00118 inline size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj);
00119
00120 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
00121 inline size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE> &obj);
00122
00123 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
00124 inline bool isMapEmpty(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj);
00125
00126 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
00127 inline bool isMapEmpty(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE> &obj);
00128 }
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE = double>
00158 class CKalmanFilterCapable : public mrpt::utils::CDebugOutputCapable
00159 {
00160 public:
00161 static inline size_t get_vehicle_size() { return VEH_SIZE; }
00162 static inline size_t get_observation_size() { return OBS_SIZE; }
00163 static inline size_t get_feature_size() { return FEAT_SIZE; }
00164 static inline size_t get_action_size() { return ACT_SIZE; }
00165 inline size_t getNumberOfLandmarksInTheMap() const { return detail::getNumberOfLandmarksInMap(*this); }
00166 inline bool isMapEmpty() const { return detail::isMapEmpty(*this); }
00167
00168
00169 typedef KFTYPE kftype;
00170 typedef CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> KFCLASS;
00171
00172
00173 typedef mrpt::dynamicsize_vector<KFTYPE> KFVector;
00174 typedef CMatrixTemplateNumeric<KFTYPE> KFMatrix;
00175
00176 typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,VEH_SIZE> KFMatrix_VxV;
00177 typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,OBS_SIZE> KFMatrix_OxO;
00178 typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,FEAT_SIZE> KFMatrix_FxF;
00179 typedef CMatrixFixedNumeric<KFTYPE,ACT_SIZE,ACT_SIZE> KFMatrix_AxA;
00180
00181 typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,OBS_SIZE> KFMatrix_VxO;
00182 typedef CMatrixFixedNumeric<KFTYPE,VEH_SIZE,FEAT_SIZE> KFMatrix_VxF;
00183
00184 typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,VEH_SIZE> KFMatrix_FxV;
00185 typedef CMatrixFixedNumeric<KFTYPE,FEAT_SIZE,OBS_SIZE> KFMatrix_FxO;
00186
00187 typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,FEAT_SIZE> KFMatrix_OxF;
00188 typedef CMatrixFixedNumeric<KFTYPE,OBS_SIZE,VEH_SIZE> KFMatrix_OxV;
00189
00190 typedef CArrayNumeric<KFTYPE,VEH_SIZE> KFArray_VEH;
00191 typedef CArrayNumeric<KFTYPE,ACT_SIZE> KFArray_ACT;
00192 typedef CArrayNumeric<KFTYPE,OBS_SIZE> KFArray_OBS;
00193 typedef typename mrpt::aligned_containers<KFArray_OBS>::vector_t vector_KFArray_OBS;
00194 typedef CArrayNumeric<KFTYPE,FEAT_SIZE> KFArray_FEAT;
00195
00196 inline size_t getStateVectorLength() const { return m_xkk.size(); }
00197
00198
00199
00200
00201 inline void getLandmarkMean(size_t idx, KFArray_FEAT &feat ) const {
00202 ASSERT_(idx<getNumberOfLandmarksInTheMap())
00203 ::memcpy(&feat[0], &m_xkk[VEH_SIZE+idx*FEAT_SIZE], FEAT_SIZE*sizeof(m_xkk[0]));
00204 }
00205
00206
00207
00208 inline void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov ) const {
00209 m_pkk.extractMatrix(VEH_SIZE+idx*FEAT_SIZE,VEH_SIZE+idx*FEAT_SIZE,feat_cov);
00210 }
00211
00212 protected:
00213
00214
00215
00216 KFVector m_xkk;
00217 KFMatrix m_pkk;
00218
00219
00220
00221 mrpt::utils::CTimeLogger m_timLogger;
00222
00223
00224
00225
00226
00227
00228
00229
00230 virtual void OnGetAction( KFArray_ACT &out_u ) const = 0;
00231
00232
00233
00234
00235
00236
00237
00238 virtual void OnTransitionModel(
00239 const KFArray_ACT &in_u,
00240 KFArray_VEH &inout_x,
00241 bool &out_skipPrediction
00242 ) const = 0;
00243
00244
00245
00246
00247
00248 virtual void OnTransitionJacobian( KFMatrix_VxV &out_F ) const
00249 {
00250 m_user_didnt_implement_jacobian=true;
00251 }
00252
00253
00254
00255 virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
00256 {
00257 std::fill_n(&out_increments[0], VEH_SIZE, 1e-6);
00258 }
00259
00260
00261
00262
00263
00264 virtual void OnTransitionNoise( KFMatrix_VxV &out_Q ) const = 0;
00265
00266
00267
00268
00269
00270
00271
00272
00273 virtual void OnPreComputingPredictions(
00274 const vector_KFArray_OBS &in_all_prediction_means,
00275 mrpt::vector_size_t &out_LM_indices_to_predict ) const
00276 {
00277
00278 const size_t N = this->getNumberOfLandmarksInTheMap();
00279 out_LM_indices_to_predict.resize(N);
00280 for (size_t i=0;i<N;i++) out_LM_indices_to_predict[i]=i;
00281 }
00282
00283
00284
00285
00286
00287 virtual void OnGetObservationNoise(KFMatrix_OxO &out_R) const = 0;
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300 virtual void OnGetObservationsAndDataAssociation(
00301 vector_KFArray_OBS &out_z,
00302 mrpt::vector_int &out_data_association,
00303 const vector_KFArray_OBS &in_all_predictions,
00304 const KFMatrix &in_S,
00305 const vector_size_t &in_lm_indices_in_S,
00306 const KFMatrix_OxO &in_R
00307 ) = 0;
00308
00309
00310
00311
00312
00313 virtual void OnObservationModel(
00314 const mrpt::vector_size_t &idx_landmarks_to_predict,
00315 vector_KFArray_OBS &out_predictions
00316 ) const = 0;
00317
00318
00319
00320
00321
00322
00323 virtual void OnObservationJacobians(
00324 const size_t &idx_landmark_to_predict,
00325 KFMatrix_OxV &Hx,
00326 KFMatrix_OxF &Hy
00327 ) const
00328 {
00329 m_user_didnt_implement_jacobian=true;
00330 }
00331
00332
00333
00334 virtual void OnObservationJacobiansNumericGetIncrements(
00335 KFArray_VEH &out_veh_increments,
00336 KFArray_FEAT &out_feat_increments ) const
00337 {
00338 std::fill_n(&out_veh_increments[0], VEH_SIZE, 1e-6);
00339 std::fill_n(&out_feat_increments[0], FEAT_SIZE, 1e-6);
00340 }
00341
00342
00343
00344 virtual void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const
00345 {
00346 A -= B;
00347 }
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362 virtual void OnInverseObservationModel(
00363 const KFArray_OBS & in_z,
00364 KFArray_FEAT & out_yn,
00365 KFMatrix_FxV & out_dyn_dxv,
00366 KFMatrix_FxO & out_dyn_dhn ) const
00367 {
00368 MRPT_START
00369 THROW_EXCEPTION("Inverse sensor model required but not implemented in derived class.")
00370 MRPT_END
00371 }
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395 virtual void OnInverseObservationModel(
00396 const KFArray_OBS & in_z,
00397 KFArray_FEAT & out_yn,
00398 KFMatrix_FxV & out_dyn_dxv,
00399 KFMatrix_FxO & out_dyn_dhn,
00400 KFMatrix_FxF & out_dyn_dhn_R_dyn_dhnT,
00401 bool & out_use_dyn_dhn_jacobian
00402 ) const
00403 {
00404 MRPT_START
00405 OnInverseObservationModel(in_z,out_yn,out_dyn_dxv,out_dyn_dhn);
00406 out_use_dyn_dhn_jacobian=true;
00407 MRPT_END
00408 }
00409
00410
00411
00412
00413
00414
00415 virtual void OnNewLandmarkAddedToMap(
00416 const size_t in_obsIdx,
00417 const size_t in_idxNewFeat )
00418 {
00419
00420 }
00421
00422
00423
00424 virtual void OnNormalizeStateVector()
00425 {
00426
00427 }
00428
00429
00430
00431 virtual void OnPostIteration()
00432 {
00433
00434 }
00435
00436
00437
00438
00439 public:
00440 CKalmanFilterCapable() {}
00441 virtual ~CKalmanFilterCapable() {}
00442
00443 mrpt::utils::CTimeLogger &getProfiler() { return m_timLogger; }
00444
00445 TKF_options KF_options;
00446
00447
00448 private:
00449
00450
00451
00452 vector_KFArray_OBS all_predictions;
00453 vector_size_t predictLMidxs;
00454 KFMatrix dh_dx;
00455 KFMatrix dh_dx_full;
00456 vector_size_t idxs;
00457 KFMatrix S;
00458 KFMatrix Pkk_subset;
00459 vector_KFArray_OBS Z;
00460 KFMatrix K;
00461 KFMatrix S_1;
00462 KFMatrix dh_dx_full_obs;
00463 KFMatrix aux_K_dh_dx;
00464
00465 protected:
00466
00467
00468
00469
00470
00471 void runOneKalmanIteration()
00472 {
00473 MRPT_START
00474
00475 m_timLogger.enable(KF_options.enable_profiler || KF_options.verbose);
00476 m_timLogger.enter("KF:complete_step");
00477
00478 ASSERT_(size_t(m_xkk.size())==m_pkk.getColCount())
00479 ASSERT_(size_t(m_xkk.size())>=VEH_SIZE)
00480
00481
00482
00483
00484 KFArray_ACT u;
00485
00486 m_timLogger.enter("KF:1.OnGetAction");
00487 OnGetAction(u);
00488 m_timLogger.leave("KF:1.OnGetAction");
00489
00490
00491 if (FEAT_SIZE) { ASSERTDEB_( (((m_xkk.size()-VEH_SIZE)/FEAT_SIZE)*FEAT_SIZE)== (m_xkk.size()-VEH_SIZE) ) }
00492
00493
00494
00495
00496 m_timLogger.enter("KF:2.prediction stage");
00497
00498 const size_t N_map = getNumberOfLandmarksInTheMap();
00499
00500 KFArray_VEH xv( &m_xkk[0] );
00501
00502 bool skipPrediction=false;
00503
00504
00505
00506 OnTransitionModel(u, xv, skipPrediction);
00507
00508 if ( !skipPrediction )
00509 {
00510
00511
00512
00513
00514 KFMatrix_VxV dfv_dxv;
00515
00516
00517 m_user_didnt_implement_jacobian=false;
00518 if (KF_options.use_analytic_transition_jacobian)
00519 OnTransitionJacobian(dfv_dxv);
00520
00521 if (m_user_didnt_implement_jacobian || !KF_options.use_analytic_transition_jacobian || KF_options.debug_verify_analytic_jacobians)
00522 {
00523 KFArray_VEH xkk_vehicle( &m_xkk[0] );
00524 KFArray_VEH xkk_veh_increments;
00525 OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
00526
00527 mrpt::math::estimateJacobian(
00528 xkk_vehicle,
00529 &KF_aux_estimate_trans_jacobian,
00530 xkk_veh_increments,
00531 std::make_pair<KFCLASS*,KFArray_ACT>(this,u),
00532 dfv_dxv);
00533
00534 if (KF_options.debug_verify_analytic_jacobians)
00535 {
00536 KFMatrix_VxV dfv_dxv_gt(UNINITIALIZED_MATRIX);
00537 OnTransitionJacobian(dfv_dxv_gt);
00538 if ((dfv_dxv-dfv_dxv_gt).Abs().sumAll()>KF_options.debug_verify_analytic_jacobians_threshold)
00539 {
00540 std::cerr << "[KalmanFilter] ERROR: User analytical transition Jacobians are wrong: \n"
00541 << " Real dfv_dxv: \n" << dfv_dxv << "\n Analytical dfv_dxv:\n" << dfv_dxv_gt << "Diff:\n" << (dfv_dxv-dfv_dxv_gt) << "\n";
00542 THROW_EXCEPTION("ERROR: User analytical transition Jacobians are wrong (More details dumped to cerr)")
00543 }
00544 }
00545
00546 }
00547
00548
00549 KFMatrix_VxV Q;
00550 OnTransitionNoise(Q);
00551
00552
00553
00554
00555
00556 m_pkk.block(0,0,VEH_SIZE,VEH_SIZE) =
00557 Q +
00558 dfv_dxv * m_pkk.block(0,0,VEH_SIZE,VEH_SIZE) * dfv_dxv.transpose();
00559
00560
00561
00562
00563
00564 KFMatrix_VxF aux;
00565 for (size_t i=0 ; i<N_map ; i++)
00566 {
00567
00568 dfv_dxv.multiply_subMatrix(
00569 m_pkk,
00570 aux,
00571 VEH_SIZE+i*FEAT_SIZE,
00572 0,
00573 FEAT_SIZE
00574 );
00575
00576 m_pkk.insertMatrix (0, VEH_SIZE+i*FEAT_SIZE, aux );
00577 m_pkk.insertMatrixTranspose(VEH_SIZE+i*FEAT_SIZE, 0 , aux );
00578 }
00579
00580
00581
00582
00583 for (size_t i=0;i<VEH_SIZE;i++)
00584 m_xkk[i]=xv[i];
00585
00586
00587 OnNormalizeStateVector();
00588
00589 }
00590
00591
00592 const double tim_pred = m_timLogger.leave("KF:2.prediction stage");
00593
00594
00595
00596
00597
00598 m_timLogger.enter("KF:3.predict all obs");
00599
00600 KFMatrix_OxO R;
00601 OnGetObservationNoise(R);
00602
00603
00604
00605 all_predictions.resize(N_map);
00606 OnObservationModel(
00607 mrpt::math::sequenceStdVec<size_t,1>(0,N_map),
00608 all_predictions);
00609
00610 const double tim_pred_obs = m_timLogger.leave("KF:3.predict all obs");
00611
00612 m_timLogger.enter("KF:4.decide pred obs");
00613
00614
00615 predictLMidxs.clear();
00616 if (FEAT_SIZE==0)
00617
00618 predictLMidxs.assign(1,0);
00619 else
00620
00621 OnPreComputingPredictions(all_predictions, predictLMidxs);
00622
00623
00624 m_timLogger.leave("KF:4.decide pred obs");
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650 m_timLogger.enter("KF:5.build Jacobians");
00651
00652 const size_t N_pred = FEAT_SIZE==0 ?
00653 1 :
00654 predictLMidxs.size();
00655
00656 dh_dx.zeros(N_pred*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_pred );
00657 dh_dx_full.zeros(N_pred*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_map );
00658
00659
00660 idxs.clear();
00661 idxs.reserve(VEH_SIZE+N_pred*FEAT_SIZE);
00662
00663 for (size_t i=0;i<VEH_SIZE;i++) idxs.push_back(i);
00664
00665 for (size_t i=0;i<N_pred;++i)
00666 {
00667 const size_t lm_idx = FEAT_SIZE==0 ? 0 : predictLMidxs[i];
00668 KFMatrix_OxV Hx(UNINITIALIZED_MATRIX);
00669 KFMatrix_OxF Hy(UNINITIALIZED_MATRIX);
00670
00671
00672 m_user_didnt_implement_jacobian=false;
00673 if (KF_options.use_analytic_transition_jacobian)
00674 OnObservationJacobians(lm_idx,Hx,Hy);
00675
00676 if (m_user_didnt_implement_jacobian || !KF_options.use_analytic_observation_jacobian || KF_options.debug_verify_analytic_jacobians)
00677 {
00678 const size_t lm_idx_in_statevector = VEH_SIZE+lm_idx*FEAT_SIZE;
00679
00680 const KFArray_VEH x_vehicle( &m_xkk[0] );
00681 const KFArray_FEAT x_feat( &m_xkk[lm_idx_in_statevector] );
00682
00683 KFArray_VEH xkk_veh_increments;
00684 KFArray_FEAT feat_increments;
00685 OnObservationJacobiansNumericGetIncrements(xkk_veh_increments, feat_increments);
00686
00687 mrpt::math::estimateJacobian(
00688 x_vehicle,
00689 &KF_aux_estimate_obs_Hx_jacobian,
00690 xkk_veh_increments,
00691 std::make_pair<KFCLASS*,size_t>(this,lm_idx),
00692 Hx);
00693
00694 ::memcpy(&m_xkk[0],&x_vehicle[0],sizeof(m_xkk[0])*VEH_SIZE);
00695
00696 mrpt::math::estimateJacobian(
00697 x_feat,
00698 &KF_aux_estimate_obs_Hy_jacobian,
00699 feat_increments,
00700 std::make_pair<KFCLASS*,size_t>(this,lm_idx),
00701 Hy);
00702
00703 ::memcpy(&m_xkk[lm_idx_in_statevector],&x_feat[0],sizeof(m_xkk[0])*FEAT_SIZE);
00704
00705 if (KF_options.debug_verify_analytic_jacobians)
00706 {
00707 KFMatrix_OxV Hx_gt(UNINITIALIZED_MATRIX);
00708 KFMatrix_OxF Hy_gt(UNINITIALIZED_MATRIX);
00709 OnObservationJacobians(lm_idx,Hx_gt,Hy_gt);
00710 if ((Hx-Hx_gt).Abs().sumAll()>KF_options.debug_verify_analytic_jacobians_threshold) {
00711 std::cerr << "[KalmanFilter] ERROR: User analytical observation Hx Jacobians are wrong: \n"
00712 << " Real Hx: \n" << Hx << "\n Analytical Hx:\n" << Hx_gt << "Diff:\n" << Hx-Hx_gt << "\n";
00713 THROW_EXCEPTION("ERROR: User analytical observation Hx Jacobians are wrong (More details dumped to cerr)")
00714 }
00715 if ((Hy-Hy_gt).Abs().sumAll()>KF_options.debug_verify_analytic_jacobians_threshold) {
00716 std::cerr << "[KalmanFilter] ERROR: User analytical observation Hy Jacobians are wrong: \n"
00717 << " Real Hy: \n" << Hy << "\n Analytical Hx:\n" << Hy_gt << "Diff:\n" << Hy-Hy_gt << "\n";
00718 THROW_EXCEPTION("ERROR: User analytical observation Hy Jacobians are wrong (More details dumped to cerr)")
00719 }
00720 }
00721 }
00722
00723 dh_dx.insertMatrix(i*OBS_SIZE,0, Hx);
00724 if (FEAT_SIZE!=0)
00725 dh_dx.insertMatrix(i*OBS_SIZE,VEH_SIZE+i*FEAT_SIZE, Hy);
00726
00727 dh_dx_full.insertMatrix(i*OBS_SIZE,0, Hx);
00728 if (FEAT_SIZE!=0)
00729 {
00730 dh_dx_full.insertMatrix(i*OBS_SIZE,VEH_SIZE+lm_idx*FEAT_SIZE, Hy);
00731
00732 for (size_t k=0;k<FEAT_SIZE;k++)
00733 idxs.push_back(k+VEH_SIZE+FEAT_SIZE*lm_idx);
00734 }
00735 }
00736 m_timLogger.leave("KF:5.build Jacobians");
00737
00738
00739
00740
00741 m_timLogger.enter("KF:6.build S");
00742
00743 S.setSize(N_pred*OBS_SIZE,N_pred*OBS_SIZE);
00744
00745
00746
00747 m_pkk.extractSubmatrixSymmetrical(idxs,Pkk_subset);
00748
00749
00750 dh_dx.multiply_HCHt(Pkk_subset,S);
00751
00752
00753 if ( FEAT_SIZE>0 )
00754 {
00755 for (size_t i=0;i<N_pred;++i)
00756 {
00757 const size_t obs_idx_off = i*OBS_SIZE;
00758 for (size_t j=0;j<OBS_SIZE;j++)
00759 for (size_t k=0;k<OBS_SIZE;k++)
00760 S.get_unsafe(obs_idx_off+j,obs_idx_off+k) += R.get_unsafe(j,k);
00761 }
00762 }
00763 else
00764 {
00765 ASSERTDEB_(S.getColCount() == OBS_SIZE );
00766 S+=R;
00767 }
00768
00769 m_timLogger.leave("KF:6.build S");
00770
00771
00772 Z.clear();
00773 vector_int data_association;
00774
00775 m_timLogger.enter("KF:7.get obs & DA");
00776
00777
00778 OnGetObservationsAndDataAssociation(
00779 Z, data_association,
00780 all_predictions, S, predictLMidxs, R
00781 );
00782
00783 ASSERTDEB_(data_association.size()==Z.size() || (data_association.empty() && FEAT_SIZE==0));
00784
00785 const double tim_obs_DA = m_timLogger.leave("KF:7.get obs & DA");
00786
00787
00788
00789
00790
00791 if ( !Z.empty() )
00792 {
00793 m_timLogger.enter("KF:8.update stage");
00794
00795 switch (KF_options.method)
00796 {
00797
00798
00799
00800 case kfEKFNaive:
00801 case kfIKFFull:
00802 {
00803
00804
00805
00806 vector_int mapIndicesForKFUpdate(data_association.size());
00807 mapIndicesForKFUpdate.resize( std::distance(mapIndicesForKFUpdate.begin(),
00808 std::remove_copy_if(
00809 data_association.begin(),
00810 data_association.end(),
00811 mapIndicesForKFUpdate.begin(),
00812 binder1st<equal_to<int> >(equal_to<int>(),-1) ) ) );
00813
00814 const size_t N_upd = (FEAT_SIZE==0) ?
00815 1 :
00816 mapIndicesForKFUpdate.size();
00817
00818
00819 const size_t nKF_iterations = (KF_options.method==kfEKFNaive) ? 1 : KF_options.IKF_iterations;
00820
00821 const KFVector xkk_0 = m_xkk;
00822
00823
00824 if (N_upd>0)
00825 {
00826 for (size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
00827 {
00828
00829 KFVector ytilde(OBS_SIZE*N_upd);
00830 size_t ytilde_idx = 0;
00831
00832
00833 dh_dx_full_obs.zeros(N_upd*OBS_SIZE, VEH_SIZE + FEAT_SIZE * N_map );
00834 KFMatrix S_observed;
00835
00836 if (FEAT_SIZE!=0)
00837 {
00838 vector_size_t S_idxs;
00839 S_idxs.reserve(OBS_SIZE*N_upd);
00840
00841 for (size_t i=0;i<data_association.size();++i)
00842 {
00843 if (data_association[i]<0) continue;
00844
00845 const size_t assoc_idx_in_map = static_cast<size_t>(data_association[i]);
00846 const size_t assoc_idx_in_pred = mrpt::utils::find_in_vector(assoc_idx_in_map, predictLMidxs);
00847 ASSERTMSG_(assoc_idx_in_pred!=string::npos, "OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!")
00848
00849
00850
00851 for (size_t k=0;k<OBS_SIZE;k++)
00852 {
00853 ::memcpy(
00854 dh_dx_full_obs.get_unsafe_row(S_idxs.size()),
00855 dh_dx_full.get_unsafe_row(assoc_idx_in_pred*OBS_SIZE + k ),
00856 sizeof(dh_dx_full(0,0))*(VEH_SIZE + FEAT_SIZE * N_map) );
00857 S_idxs.push_back(assoc_idx_in_pred*OBS_SIZE+k);
00858 }
00859
00860
00861 KFArray_OBS ytilde_i = Z[i];
00862 OnSubstractObservationVectors(ytilde_i,all_predictions[predictLMidxs[assoc_idx_in_pred]]);
00863 for (size_t k=0;k<OBS_SIZE;k++)
00864 ytilde[ytilde_idx++] = ytilde_i[k];
00865 }
00866
00867 S.extractSubmatrixSymmetrical(S_idxs,S_observed);
00868 }
00869 else
00870 {
00871 ASSERT_(Z.size()==1 && all_predictions.size()==1)
00872
00873 dh_dx_full_obs = dh_dx_full;
00874 KFArray_OBS ytilde_i = Z[0];
00875 OnSubstractObservationVectors(ytilde_i,all_predictions[0]);
00876 for (size_t k=0;k<OBS_SIZE;k++)
00877 ytilde[ytilde_idx++] = ytilde_i[k];
00878
00879 S_observed = S;
00880 }
00881
00882
00883
00884 m_timLogger.enter("KF:8.update stage:1.FULLKF:build K");
00885
00886 K.setSize(m_pkk.getRowCount(), S_observed.getColCount() );
00887
00888
00889 K.multiply_ABt(m_pkk, dh_dx_full_obs);
00890
00891
00892 S_observed.inv(S_1);
00893 K*=S_1;
00894
00895 m_timLogger.leave("KF:8.update stage:1.FULLKF:build K");
00896
00897
00898 if (nKF_iterations==1)
00899 {
00900 m_timLogger.enter("KF:8.update stage:2.FULLKF:update xkk");
00901 m_xkk += K * ytilde;
00902 m_timLogger.leave("KF:8.update stage:2.FULLKF:update xkk");
00903 }
00904 else
00905 {
00906 m_timLogger.enter("KF:8.update stage:2.FULLKF:iter.update xkk");
00907
00908 KFVector HAx_column;
00909 dh_dx_full_obs.multiply_Ab( m_xkk - xkk_0, HAx_column);
00910
00911 m_xkk = xkk_0;
00912 K.multiply_Ab(
00913 (ytilde-HAx_column),
00914 m_xkk,
00915 true
00916 );
00917
00918 m_timLogger.leave("KF:8.update stage:2.FULLKF:iter.update xkk");
00919 }
00920
00921
00922
00923 if (IKF_iteration == (nKF_iterations-1) )
00924 {
00925 m_timLogger.enter("KF:8.update stage:3.FULLKF:update Pkk");
00926
00927
00928
00929
00930
00931
00932 aux_K_dh_dx.multiply(K,dh_dx_full_obs);
00933
00934
00935 const size_t stat_len = aux_K_dh_dx.getColCount();
00936 for (size_t r=0;r<stat_len;r++)
00937 for (size_t c=0;c<stat_len;c++)
00938 if (r==c)
00939 aux_K_dh_dx.get_unsafe(r,c)=-aux_K_dh_dx.get_unsafe(r,c) + kftype(1);
00940 else aux_K_dh_dx.get_unsafe(r,c)=-aux_K_dh_dx.get_unsafe(r,c);
00941
00942 m_pkk.multiply_result_is_symmetric(aux_K_dh_dx, m_pkk );
00943
00944 m_timLogger.leave("KF:8.update stage:3.FULLKF:update Pkk");
00945 }
00946 }
00947 }
00948 }
00949 break;
00950
00951
00952
00953
00954 case kfEKFAlaDavison:
00955 {
00956
00957 for (size_t obsIdx=0;obsIdx<Z.size();obsIdx++)
00958 {
00959
00960 bool doit;
00961 size_t idxInTheFilter=0;
00962
00963 if (data_association.empty())
00964 {
00965 doit = true;
00966 }
00967 else
00968 {
00969 doit = data_association[obsIdx] >= 0;
00970 if (doit)
00971 idxInTheFilter = data_association[obsIdx];
00972 }
00973
00974 if ( doit )
00975 {
00976 m_timLogger.enter("KF:8.update stage:1.ScalarAtOnce.prepare");
00977
00978
00979 const size_t idx_off = VEH_SIZE + idxInTheFilter*FEAT_SIZE;
00980
00981
00982 vector_KFArray_OBS pred_obs;
00983 OnObservationModel( vector_size_t(1,idxInTheFilter),pred_obs);
00984 ASSERTDEB_(pred_obs.size()==1);
00985
00986
00987 KFArray_OBS ytilde = Z[obsIdx];
00988 OnSubstractObservationVectors(ytilde, pred_obs[0]);
00989
00990
00991
00992
00993
00994 KFMatrix_OxV Hx(UNINITIALIZED_MATRIX);
00995 KFMatrix_OxF Hy(UNINITIALIZED_MATRIX);
00996 const size_t i_idx_in_preds = mrpt::utils::find_in_vector(idxInTheFilter,predictLMidxs);
00997 ASSERTMSG_(i_idx_in_preds!=string::npos, "OnPreComputingPredictions() didn't recommend the prediction of a landmark which has been actually observed!")
00998 dh_dx.extractMatrix(i_idx_in_preds*OBS_SIZE,0, Hx);
00999 dh_dx.extractMatrix(i_idx_in_preds*OBS_SIZE,VEH_SIZE+i_idx_in_preds*OBS_SIZE, Hy);
01000
01001
01002 m_timLogger.leave("KF:8.update stage:1.ScalarAtOnce.prepare");
01003
01004
01005 for (size_t j=0;j<OBS_SIZE;j++)
01006 {
01007 m_timLogger.enter("KF:8.update stage:2.ScalarAtOnce.update");
01008
01009
01010
01011
01012
01013
01014
01015
01016
01017
01018
01019
01020
01021 #if defined(_DEBUG)
01022 {
01023
01024 for (size_t a=0;a<OBS_SIZE;a++)
01025 for (size_t b=0;b<OBS_SIZE;b++)
01026 if ( a!=b )
01027 if (R(a,b)!=0)
01028 THROW_EXCEPTION("This KF algorithm assumes independent noise components in the observation (matrix R). Select another KF algorithm.")
01029 }
01030 #endif
01031
01032 KFTYPE Sij = R.get_unsafe(j,j);
01033
01034
01035 for (size_t k=0;k<VEH_SIZE;k++)
01036 {
01037 KFTYPE accum = 0;
01038 for (size_t q=0;q<VEH_SIZE;q++)
01039 accum += Hx.get_unsafe(j,q) * m_pkk.get_unsafe(q,k);
01040 Sij+= Hx.get_unsafe(j,k) * accum;
01041 }
01042
01043
01044 KFTYPE term2=0;
01045 for (size_t k=0;k<VEH_SIZE;k++)
01046 {
01047 KFTYPE accum = 0;
01048 for (size_t q=0;q<FEAT_SIZE;q++)
01049 accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,k);
01050 term2+= Hx.get_unsafe(j,k) * accum;
01051 }
01052 Sij += 2 * term2;
01053
01054
01055 for (size_t k=0;k<FEAT_SIZE;k++)
01056 {
01057 KFTYPE accum = 0;
01058 for (size_t q=0;q<FEAT_SIZE;q++)
01059 accum += Hy.get_unsafe(j,q) * m_pkk.get_unsafe(idx_off+q,idx_off+k);
01060 Sij+= Hy.get_unsafe(j,k) * accum;
01061 }
01062
01063
01064
01065 size_t N = m_pkk.getColCount();
01066 vector<KFTYPE> Kij( N );
01067
01068 for (size_t k=0;k<N;k++)
01069 {
01070 KFTYPE K_tmp = 0;
01071
01072
01073 size_t q;
01074 for (q=0;q<VEH_SIZE;q++)
01075 K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(j,q);
01076
01077
01078 for (q=0;q<FEAT_SIZE;q++)
01079 K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(j,q);
01080
01081 Kij[k] = K_tmp / Sij;
01082 }
01083
01084
01085
01086
01087 for (size_t k=0;k<N;k++)
01088 m_xkk[k] += Kij[k] * ytilde[j];
01089
01090
01091
01092 {
01093 for (size_t k=0;k<N;k++)
01094 {
01095 for (size_t q=k;q<N;q++)
01096 {
01097 m_pkk(k,q) -= Sij * Kij[k] * Kij[q];
01098
01099 m_pkk(q,k) = m_pkk(k,q);
01100 }
01101
01102 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
01103 if (m_pkk(k,k)<0)
01104 {
01105 m_pkk.saveToTextFile("Pkk_err.txt");
01106 mrpt::system::vectorToTextFile(Kij,"Kij.txt");
01107 ASSERT_(m_pkk(k,k)>0)
01108 }
01109 #endif
01110 }
01111 }
01112
01113
01114 m_timLogger.leave("KF:8.update stage:2.ScalarAtOnce.update");
01115 }
01116 }
01117 }
01118 }
01119 break;
01120
01121
01122
01123
01124 case kfIKF:
01125 {
01126 THROW_EXCEPTION("IKF scalar by scalar not implemented yet.");
01127 #if 0
01128 KFMatrix h,Hx,Hy;
01129
01130
01131 size_t nKF_iterations = KF_options.IKF_iterations;
01132
01133
01134 KFMatrix *saved_Pkk=NULL;
01135 if (nKF_iterations>1)
01136 {
01137
01138 saved_Pkk = new KFMatrix( m_pkk );
01139 }
01140
01141 KFVector xkk_0 = m_xkk;
01142 KFVector xkk_next_iter = m_xkk;
01143
01144
01145 for (size_t IKF_iteration=0;IKF_iteration<nKF_iterations;IKF_iteration++)
01146 {
01147
01148
01149
01150 if (IKF_iteration>0)
01151 {
01152 m_pkk = *saved_Pkk;
01153 xkk_next_iter = xkk_0;
01154 }
01155
01156
01157 for (size_t obsIdx=0;obsIdx<Z.getRowCount();obsIdx++)
01158 {
01159
01160 bool doit;
01161 size_t idxInTheFilter=0;
01162
01163 if (data_association.empty())
01164 {
01165 doit = true;
01166 }
01167 else
01168 {
01169 doit = data_association[obsIdx] >= 0;
01170 if (doit)
01171 idxInTheFilter = data_association[obsIdx];
01172 }
01173
01174 if ( doit )
01175 {
01176
01177 const size_t idx_off = VEH_SIZE + idxInTheFilter *FEAT_SIZE;
01178 const size_t R_row_offset = obsIdx*OBS_SIZE;
01179
01180
01181 KFVector ytilde;
01182 OnObservationModelAndJacobians(
01183 Z,
01184 data_association,
01185 false,
01186 (int)obsIdx,
01187 ytilde,
01188 Hx,
01189 Hy );
01190
01191 ASSERTDEB_(ytilde.size() == OBS_SIZE )
01192 ASSERTDEB_(Hx.getRowCount() == OBS_SIZE )
01193 ASSERTDEB_(Hx.getColCount() == VEH_SIZE )
01194
01195 if (FEAT_SIZE>0)
01196 {
01197 ASSERTDEB_(Hy.getRowCount() == OBS_SIZE )
01198 ASSERTDEB_(Hy.getColCount() == FEAT_SIZE )
01199 }
01200
01201
01202
01203
01204
01205
01206
01207
01208
01209
01210
01211
01212
01213
01214
01215
01216
01217
01218
01219
01220
01221 KFMatrix Si(OBS_SIZE,OBS_SIZE);
01222 R.extractMatrix(R_row_offset,0, Si);
01223
01224 size_t k;
01225 KFMatrix term(OBS_SIZE,OBS_SIZE);
01226
01227
01228 Hx.multiply_HCHt(
01229 m_pkk,
01230 Si,
01231 true,
01232 0,
01233 true
01234 );
01235
01236
01237
01238 KFMatrix Pyix( FEAT_SIZE, VEH_SIZE );
01239 m_pkk.extractMatrix(idx_off,0, Pyix);
01240
01241 term.multiply_ABCt( Hy, Pyix, Hx );
01242 Si.add_AAt( term );
01243
01244
01245 Hy.multiply_HCHt(
01246 m_pkk,
01247 Si,
01248 true,
01249 idx_off,
01250 true
01251 );
01252
01253
01254 KFMatrix Si_1(OBS_SIZE,OBS_SIZE);
01255
01256
01257
01258 size_t N = m_pkk.getColCount();
01259
01260 KFMatrix Ki( N, OBS_SIZE );
01261
01262 for (k=0;k<N;k++)
01263 {
01264 size_t q;
01265
01266 for (size_t c=0;c<OBS_SIZE;c++)
01267 {
01268 KFTYPE K_tmp = 0;
01269
01270
01271 for (q=0;q<VEH_SIZE;q++)
01272 K_tmp+= m_pkk.get_unsafe(k,q) * Hx.get_unsafe(c,q);
01273
01274
01275 for (q=0;q<FEAT_SIZE;q++)
01276 K_tmp+= m_pkk.get_unsafe(k,idx_off+q) * Hy.get_unsafe(c,q);
01277
01278 Ki.set_unsafe(k,c, K_tmp);
01279 }
01280 }
01281
01282 Ki.multiply(Ki, Si.inv() );
01283
01284
01285
01286 if (nKF_iterations==1)
01287 {
01288
01289
01290 for (k=0;k<N;k++)
01291 for (size_t q=0;q<OBS_SIZE;q++)
01292 m_xkk[k] += Ki.get_unsafe(k,q) * ytilde[q];
01293 }
01294 else
01295 {
01296
01297 mrpt::dynamicsize_vector<KFTYPE> HAx(OBS_SIZE);
01298 size_t o,q;
01299
01300 for (o=0;o<OBS_SIZE;o++)
01301 {
01302 KFTYPE tmp = 0;
01303 for (q=0;q<VEH_SIZE;q++)
01304 tmp += Hx.get_unsafe(o,q) * (xkk_0[q] - m_xkk[q]);
01305
01306 for (q=0;q<FEAT_SIZE;q++)
01307 tmp += Hy.get_unsafe(o,q) * (xkk_0[idx_off+q] - m_xkk[idx_off+q]);
01308
01309 HAx[o] = tmp;
01310 }
01311
01312
01313 for (o=0;o<OBS_SIZE;o++)
01314 HAx[o] = ytilde[o] - HAx[o];
01315
01316
01317
01318 for (k=0;k<N;k++)
01319 {
01320 KFTYPE tmp = xkk_next_iter[k];
01321
01322 for (o=0;o<OBS_SIZE;o++)
01323 tmp += Ki.get_unsafe(k,o) * HAx[o];
01324
01325 xkk_next_iter[k] = tmp;
01326 }
01327 }
01328
01329
01330
01331
01332 {
01333
01334 Ki.multiplyByMatrixAndByTransposeNonSymmetric(
01335 Si,
01336 m_pkk,
01337 true,
01338 true);
01339
01340 m_pkk.force_symmetry();
01341
01342
01343
01344
01345
01346
01347
01348
01349
01350
01351
01352
01353
01354
01355
01356
01357
01358
01359
01360
01361 }
01362
01363 }
01364
01365 }
01366
01367
01368 if (nKF_iterations>1)
01369 {
01370 #if 0
01371 cout << "IKF iter: " << IKF_iteration << " -> " << xkk_next_iter << endl;
01372 #endif
01373 m_xkk = xkk_next_iter;
01374 }
01375
01376 }
01377
01378
01379 if (saved_Pkk) delete saved_Pkk;
01380
01381 #endif
01382 }
01383 break;
01384
01385 default:
01386 THROW_EXCEPTION("Invalid value of options.KF_method");
01387 }
01388
01389 }
01390
01391 const double tim_update = m_timLogger.leave("KF:8.update stage");
01392
01393 OnNormalizeStateVector();
01394
01395
01396
01397
01398 if (!data_association.empty())
01399 {
01400 detail::CRunOneKalmanIteration_addNewLandmarks()(*this, Z, data_association,R);
01401 }
01402
01403 if (KF_options.verbose)
01404 {
01405 printf_debug("[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: %.2fms\n",
01406 static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
01407 1e3*tim_pred,
01408 1e3*tim_pred_obs,
01409 1e3*tim_obs_DA,
01410 1e3*tim_update
01411 );
01412 }
01413
01414
01415 OnPostIteration();
01416
01417 m_timLogger.leave("KF:complete_step");
01418
01419 MRPT_END
01420
01421 }
01422
01423
01424
01425 private:
01426 mutable bool m_user_didnt_implement_jacobian;
01427
01428
01429 static void KF_aux_estimate_trans_jacobian(
01430 const KFArray_VEH &x,
01431 const std::pair<KFCLASS*,KFArray_ACT> &dat,
01432 KFArray_VEH &out_x)
01433 {
01434 bool dumm;
01435 out_x=x;
01436 dat.first->OnTransitionModel(dat.second,out_x, dumm);
01437 }
01438 static void KF_aux_estimate_obs_Hx_jacobian(
01439 const KFArray_VEH &x,
01440 const std::pair<KFCLASS*,size_t> &dat,
01441 KFArray_OBS &out_x)
01442 {
01443 vector_size_t idxs_to_predict(1,dat.second);
01444 vector_KFArray_OBS prediction;
01445
01446 ::memcpy(&dat.first->m_xkk[0],&x[0],sizeof(x[0])*VEH_SIZE);
01447 dat.first->OnObservationModel(idxs_to_predict,prediction);
01448 ASSERTDEB_(prediction.size()==1);
01449 out_x=prediction[0];
01450 }
01451 static void KF_aux_estimate_obs_Hy_jacobian(
01452 const KFArray_FEAT &x,
01453 const std::pair<KFCLASS*,size_t> &dat,
01454 KFArray_OBS &out_x)
01455 {
01456 vector_size_t idxs_to_predict(1,dat.second);
01457 vector_KFArray_OBS prediction;
01458 const size_t lm_idx_in_statevector = VEH_SIZE+FEAT_SIZE*dat.second;
01459
01460 ::memcpy(&dat.first->m_xkk[lm_idx_in_statevector],&x[0],sizeof(x[0])*FEAT_SIZE);
01461 dat.first->OnObservationModel(idxs_to_predict,prediction);
01462 ASSERTDEB_(prediction.size()==1);
01463 out_x=prediction[0];
01464 }
01465
01466
01467 friend struct detail::CRunOneKalmanIteration_addNewLandmarks;
01468
01469 };
01470
01471 namespace detail
01472 {
01473
01474 struct CRunOneKalmanIteration_addNewLandmarks {
01475 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
01476 void operator()(
01477 CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj,
01478 const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE>::vector_KFArray_OBS & Z,
01479 const vector_int &data_association,
01480 const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE>::KFMatrix_OxO &R
01481 )
01482 {
01483 typedef CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> KF;
01484
01485 for (size_t idxObs=0;idxObs<Z.size();idxObs++)
01486 {
01487
01488 if ( data_association[idxObs] < 0 )
01489 {
01490 obj.m_timLogger.enter("KF:9.create new LMs");
01491
01492
01493
01494 ASSERTDEB_(FEAT_SIZE>0)
01495 ASSERTDEB_( 0 == ((obj.m_xkk.size() - VEH_SIZE) % FEAT_SIZE) )
01496
01497 const size_t newIndexInMap = (obj.m_xkk.size() - VEH_SIZE) / FEAT_SIZE;
01498
01499
01500 typename KF::KFArray_FEAT yn;
01501 typename KF::KFMatrix_FxV dyn_dxv;
01502 typename KF::KFMatrix_FxO dyn_dhn;
01503 typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
01504 bool use_dyn_dhn_jacobian=true;
01505
01506
01507 obj.OnInverseObservationModel(
01508 Z[idxObs],
01509 yn,
01510 dyn_dxv,
01511 dyn_dhn,
01512 dyn_dhn_R_dyn_dhnT,
01513 use_dyn_dhn_jacobian );
01514
01515
01516 obj.OnNewLandmarkAddedToMap(
01517 idxObs,
01518 newIndexInMap
01519 );
01520
01521 ASSERTDEB_( yn.size() == FEAT_SIZE )
01522
01523
01524 size_t q;
01525 size_t idx = obj.m_xkk.size();
01526 obj.m_xkk.resize( obj.m_xkk.size() + FEAT_SIZE );
01527
01528 for (q=0;q<FEAT_SIZE;q++)
01529 obj.m_xkk[idx+q] = yn[q];
01530
01531
01532
01533
01534 ASSERTDEB_( obj.m_pkk.getColCount()==idx && obj.m_pkk.getRowCount()==idx );
01535
01536 obj.m_pkk.setSize( idx+FEAT_SIZE,idx+FEAT_SIZE );
01537
01538
01539
01540 typename KF::KFMatrix_VxV Pxx;
01541 obj.m_pkk.extractMatrix(0,0,Pxx);
01542 typename KF::KFMatrix_FxV Pxyn;
01543 Pxyn.multiply( dyn_dxv, Pxx );
01544
01545 obj.m_pkk.insertMatrix( idx,0, Pxyn );
01546 obj.m_pkk.insertMatrixTranspose( 0,idx, Pxyn );
01547
01548
01549
01550 const size_t nLMs = (idx-VEH_SIZE)/FEAT_SIZE;
01551 for (q=0;q<nLMs;q++)
01552 {
01553 typename KF::KFMatrix_VxF P_x_yq(UNINITIALIZED_MATRIX);
01554 obj.m_pkk.extractMatrix(0,VEH_SIZE+q*FEAT_SIZE,P_x_yq) ;
01555
01556 typename KF::KFMatrix_FxF P_cross(UNINITIALIZED_MATRIX);
01557 P_cross.multiply(dyn_dxv, P_x_yq );
01558
01559 obj.m_pkk.insertMatrix(idx,VEH_SIZE+q*FEAT_SIZE, P_cross );
01560 obj.m_pkk.insertMatrixTranspose(VEH_SIZE+q*FEAT_SIZE,idx, P_cross );
01561 }
01562
01563
01564
01565
01566 typename KF::KFMatrix_FxF P_yn_yn(UNINITIALIZED_MATRIX);
01567 dyn_dxv.multiply_HCHt(Pxx, P_yn_yn);
01568 if (use_dyn_dhn_jacobian)
01569 dyn_dhn.multiply_HCHt(R, P_yn_yn, true);
01570 else P_yn_yn+=dyn_dhn_R_dyn_dhnT;
01571
01572 obj.m_pkk.insertMatrix(idx,idx, P_yn_yn );
01573
01574 obj.m_timLogger.leave("KF:9.create new LMs");
01575 }
01576 }
01577 }
01578
01579 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
01580 void operator()(
01581 CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE> &obj,
01582 const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE>::vector_KFArray_OBS & Z,
01583 const vector_int &data_association,
01584 const typename CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /* FEAT_SIZE=0 */,ACT_SIZE,KFTYPE>::KFMatrix_OxO &R
01585 )
01586 {
01587
01588 }
01589
01590 };
01591
01592 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
01593 inline size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj)
01594 {
01595 return (obj.getStateVectorLength()-VEH_SIZE)/FEAT_SIZE;
01596 }
01597
01598 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
01599 inline size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE> &obj)
01600 {
01601 return 0;
01602 }
01603
01604 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE>
01605 inline bool isMapEmpty(const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,FEAT_SIZE,ACT_SIZE,KFTYPE> &obj)
01606 {
01607 return (obj.getStateVectorLength()==VEH_SIZE);
01608 }
01609
01610 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
01611 inline bool isMapEmpty (const CKalmanFilterCapable<VEH_SIZE,OBS_SIZE,0 /*FEAT_SIZE*/,ACT_SIZE,KFTYPE> &obj)
01612 {
01613 return true;
01614 }
01615
01616 }
01617
01618 }
01619 }
01620
01621 #endif