bayes++ Library Introduction Learning familiar UKF related classes

Ukf-slam is a relatively popular slam scheme. Compared with EKF-SLAM,UKF, the unscented transform is substituted for the linearization of the EKF, thus it has higher precision. The related classes of UKF implementation are given in the UNSFLT.HPP in the bayes++ library.

namespaceBayesian_filter the {    +     A classUnscented_predict_model: PublicPredict_model_base the /*Specific unscented prediction model for Additive noise * x (K|k-1) = f (x (k-1|k-1)) + W (x (k)) * * Un Scented filter requires * f the function part of the NON-LINEAR model * Q The covariance of the additive W (x (k)), W is specifically allow-to-be-a function of state*/    the {    -  Public:   WuyiUnscented_predict_model (std::size_t q_size) the     {    -q_unscented =q_size;  Wu     }    -     About     Virtual Constfm::vec& F (Constfm::vec& x)Const=0;  $     //functional part of additive model    -     //note:reference return value as a speed optimisation, must is copied by caller.    -     -     Virtual Constfm::symmatrix& Q (Constfm::vec& x)Const=0;  A     //covariance of additive noise    +     //note:reference return value as a speed optimisation, must is copied by caller.    the Private:    -FriendclassUnscented_filter;//Filter implementation need to know noise size    $std::size_t q_unscented;  the };  the     the     the classUnscented_scheme: PublicLinrz_kalman_filter, PublicFunctional_filter - {    in Private:    thestd::size_t Q_max;//Maximum Size allocated for noise model, constructed before XX    the  Public:    AboutFm::colmatrix XX;//unscented form of state, with associated Kappa    theFloat Kappa;  the     theUnscented_scheme (std::size_t x_size, std::size_t z_initialsize =0);  +unscented_scheme&operator= (Constunscented_scheme&);  -     //optimise copy assignment to only copy the filter state    the    Bayi     voidinit ();  the     voidinit_xx ();  the     voidupdate ();  -     voidupdate_xx (Float kappa);  -     the     voidPredict (unscented_predict_model&f);  the     //Efficient unscented Prediction    the     voidPredict (functional_predict_model&f);  the     voidPredict (additive_predict_model&f);  -Float Predict (linrz_predict_model&f) the{//Adapt to use the more general additive model    thePredict (static_cast<additive_predict_model&>(f));  the         return 1.;//Always well condition for additive predict   94     }    the         theFloat observe (uncorrelated_additive_observe_model& h,Constfm::vec&z);  theFloat observe (correlated_additive_observe_model& h,Constfm::vec&z); 98     //unscented Filter implements general additive observe models    About        -Float observe (linrz_uncorrelated_observe_model& h,Constfm::vec&z)101{//Adapt to use the more general additive model  102         returnObserve (static_cast<uncorrelated_additive_observe_model&>(h), z); 103     }  104Float observe (linrz_correlated_observe_model& h,Constfm::vec&z) the{//Adapt to use the more general additive model  106         returnObserve (static_cast<correlated_additive_observe_model&>(h), z); 107     }  108   109  Public://Exposed numerical Results   theFm::vec s;//Innovation  111Fm::symmatrix S, SI;//innovation covariance and inverse   the   113 protected:   the     VirtualFloat Predict_kappa (std::size_t size)Const;  the     VirtualFloat Observe_kappa (std::size_t size)Const;  the     /*unscented Kappa Values 117 default uses the rule which minimise mean squared error of 4th order term 118 */  119    - protected://Allow fast operation if z_size remains constant  121std::size_t last_z_size; 122     voidobserve_size (std::size_t z_size); 123   124 Private:   the     voidUnscented (fm::colmatrix& XX,Constfm::vec& x,Constfm::symmatrix&X, Float scale); 126     /*determine unscented points for a distribution*/  127std::size_t x_size;  -std::size_t xx_size;//2*x_size+1  129    the protected://permanently allocated temps  131Fm::colmatrix FXX;  the }; 133   134   135}//namespace  136 #endif

bayes++ Library Introduction Learning familiar UKF related classes