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

　　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