UKF-SLAM是一种比较流行SLAM方案。相比EKF-SLAM，UKF利用unscented transform代替了EKF的线性化趋近，因而具有更高的精度。Bayes++库中的unsFlt.hpp中给出了UKF实现的相关类。

       

namespace Bayesian_filter   39 {   40    41 class Unscented_predict_model : public Predict_model_base   42 /* Specific Unscented prediction model for Additive noise   43  *  x(k|k-1) = f(x(k-1|k-1)) + w(x(k))   44  *   45  * Unscented filter requires   46  *  f the function part of the non-linear model   47  *  Q the covariance of the additive w(x(k)), w is specifically allow to be a function of state   48  */   49 {   50 public:   51     Unscented_predict_model (std::size_t q_size)   52     {   53         q_unscented = q_size;   54     }   55    56     virtual const FM::Vec& f(const FM::Vec& x) const = 0;   57     // Functional part of additive model   58     // Note: Reference return value as a speed optimisation, MUST be copied by caller.   59    60     virtual const FM::SymMatrix& Q(const FM::Vec& x) const = 0;   61     // Covariance of additive noise   62     // Note: Reference return value as a speed optimisation, MUST be copied by caller.   63 private:   64     friend class Unscented_filter;  // Filter implementation need to know noise size   65     std::size_t q_unscented;   66 };   67    68    69 class Unscented_scheme : public Linrz_kalman_filter, public Functional_filter   70 {   71 private:   72     std::size_t q_max;          // Maximum size allocated for noise model, constructed before XX   73 public:   74     FM::ColMatrix XX;       // Unscented form of state, with associated Kappa   75     Float kappa;   76    77     Unscented_scheme (std::size_t x_size, std::size_t z_initialsize = 0);   78     Unscented_scheme& operator= (const Unscented_scheme&);   79     // Optimise copy assignment to only copy filter state   80    81     void init ();   82     void init_XX ();   83     void update ();   84     void update_XX (Float kappa);   85    86     void predict (Unscented_predict_model& f);   87     // Efficient Unscented prediction    88     void predict (Functional_predict_model& f);   89     void predict (Additive_predict_model& f);   90     Float predict (Linrz_predict_model& f)   91     {   // Adapt to use the more general additive model   92         predict(static_cast
     
      (f));   93         return 1.;      // Always well condition for additive predict   94     }   95        96     Float observe (Uncorrelated_additive_observe_model& h, const FM::Vec& z);   97     Float observe (Correlated_additive_observe_model& h, const FM::Vec& z);   98     // Unscented filter implements general additive observe models   99       100     Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z)  101     {   // Adapt to use the more general additive model  102         return observe (static_cast
      
       (h),z);  103     }  104     Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z)  105     {   // Adapt to use the more general additive model  106         return observe (static_cast
       
        (h),z);  107     }  108   109 public:                     // Exposed Numerical Results  110     FM::Vec s;                  // Innovation  111     FM::SymMatrix S, SI;        // Innovation Covariance and Inverse  112   113 protected:  114     virtual Float predict_Kappa (std::size_t size) const;  115     virtual Float observe_Kappa (std::size_t size) const;  116     /* Unscented Kappa values  117        default uses the rule which minimise mean squared error of 4th order term  118     */  119   120 protected:                  // allow fast operation if z_size remains constant  121     std::size_t last_z_size;  122     void observe_size (std::size_t z_size);  123   124 private:  125     void unscented (FM::ColMatrix& XX, const FM::Vec& x, const FM::SymMatrix& X, Float scale);  126     /* Determine Unscented points for a distribution */  127     std::size_t x_size;  128     std::size_t XX_size;    // 2*x_size+1  129   130 protected:                  // Permanently allocated temps  131     FM::ColMatrix fXX;  132 };  133   134   135 }//namespace  136 #endif