Bayes++ Library入门学习之熟悉UKF相关类

Posted 2020-08-16 Curnane

　　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<Additive_predict_model&>(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<Uncorrelated_additive_observe_model&>(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<Correlated_additive_observe_model&>(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