1 | // %pacpus:license{
|
---|
2 | // This file is part of the PACPUS framework distributed under the
|
---|
3 | // CECILL-C License, Version 1.0.
|
---|
4 | // %pacpus:license}
|
---|
5 | /// @file
|
---|
6 | /// @author Firstname Surname <firstname.surname@utc.fr>
|
---|
7 | /// @date Month, Year
|
---|
8 | /// @version $Id: gaussian_sum_filtering.hpp 76 2013-01-10 17:05:10Z kurdejma $
|
---|
9 | /// @copyright Copyright (c) UTC/CNRS Heudiasyc 2006 - 2013. All rights reserved.
|
---|
10 | /// @brief Brief description.
|
---|
11 | ///
|
---|
12 | /// Detailed description.
|
---|
13 |
|
---|
14 | #ifndef __GAUSSIAN_FILTERING_BASE__
|
---|
15 | #define __GAUSSIAN_FILTERING_BASE__
|
---|
16 |
|
---|
17 | #include "kalman_filtering.hpp"
|
---|
18 | #include "../math/pdf.hpp"
|
---|
19 |
|
---|
20 | #include <vector>
|
---|
21 |
|
---|
22 | namespace filter{
|
---|
23 |
|
---|
24 | namespace gaussiansum{
|
---|
25 |
|
---|
26 | using namespace math;
|
---|
27 | // using namespace rng;
|
---|
28 | using namespace ublas;
|
---|
29 |
|
---|
30 | /*!
|
---|
31 | * \class Gaussian
|
---|
32 | * \brief This class describes a basic weighted kalman state
|
---|
33 | */
|
---|
34 | class Gaussian : public filter::kalman::State{
|
---|
35 | public :
|
---|
36 |
|
---|
37 | /*!
|
---|
38 | * \brief Constructor
|
---|
39 | * \param state_size : Size of the state vector
|
---|
40 | * \param weight_ : Initial weight
|
---|
41 | */
|
---|
42 | Gaussian(const size_t &state_size, const double &weight_){
|
---|
43 | filter::kalman::State::Allocator(state_size);
|
---|
44 | weight=weight_;
|
---|
45 | }
|
---|
46 |
|
---|
47 | /**
|
---|
48 | * \brief Constructor
|
---|
49 | * \param state_size : Size of the state vector
|
---|
50 | */
|
---|
51 |
|
---|
52 | Gaussian(const size_t &state_size){
|
---|
53 | filter::kalman::State::Allocator(state_size);
|
---|
54 | weight=0;
|
---|
55 | }
|
---|
56 |
|
---|
57 | double weight; /*!< Weight of the Kalman state */
|
---|
58 |
|
---|
59 | /*
|
---|
60 | protected :*/
|
---|
61 |
|
---|
62 | // Gaussian:weight(1){};
|
---|
63 | };
|
---|
64 |
|
---|
65 |
|
---|
66 | /*!
|
---|
67 | * \class GaussianSet
|
---|
68 | * \brief This method describes a set of gaussians \n
|
---|
69 | * A set of gaussains is reprented by a vector of weighted kalman state \n
|
---|
70 | * somes methods can be applied to the set of particles like :\n
|
---|
71 | * estimate computation, resampling scheme or normalization method \n
|
---|
72 | */
|
---|
73 | template<class G=Gaussian> class GaussianSet {
|
---|
74 | public :
|
---|
75 |
|
---|
76 | /** Normalize the weights of gaussian states*/
|
---|
77 | void NormalizeWeights();
|
---|
78 |
|
---|
79 | /*!
|
---|
80 | * \brief Allocate the set of gaussians
|
---|
81 | * \param ngaussian : number of gaussians in the set
|
---|
82 | */
|
---|
83 | void Allocator(const size_t &ngaussian);
|
---|
84 |
|
---|
85 | /*!
|
---|
86 | * \brief Compute the estimate
|
---|
87 | * \return Vector
|
---|
88 | */
|
---|
89 | Vector Estimate();
|
---|
90 |
|
---|
91 | /*!
|
---|
92 | * \brief Destructor
|
---|
93 | */
|
---|
94 | ~GaussianSet(){}
|
---|
95 |
|
---|
96 |
|
---|
97 | std::vector<G> gaussians; /*!< gaussian states */
|
---|
98 |
|
---|
99 |
|
---|
100 | };
|
---|
101 |
|
---|
102 |
|
---|
103 | template<class G> void GaussianSet<G>::NormalizeWeights(){
|
---|
104 | double sum=0;
|
---|
105 | for(typename std::vector<G>::iterator I=gaussians.begin();I!=gaussians.end();I++)
|
---|
106 | sum+=(*I).weight;
|
---|
107 |
|
---|
108 | if(sum==0){throw filter_error("Gaussian set normalization weight sum =0");}
|
---|
109 | for(typename std::vector<G>::iterator I=gaussians.begin();I!=gaussians.end();I++)
|
---|
110 | (*I).weight/=sum;
|
---|
111 | }
|
---|
112 |
|
---|
113 |
|
---|
114 | template<class G> void GaussianSet<G>::Allocator(const size_t &ngaussian){
|
---|
115 | gaussians.reserve(ngaussian);
|
---|
116 | }
|
---|
117 |
|
---|
118 | template<class G> Vector GaussianSet<G>::Estimate(){
|
---|
119 | Vector estimate = ZeroVector(gaussians[0].X.size());
|
---|
120 | for(typename std::vector<G>::iterator I=gaussians.begin();I!=gaussians.end();I++)
|
---|
121 | estimate+=(*I).X*(*I).weight;
|
---|
122 | return estimate;
|
---|
123 | }
|
---|
124 |
|
---|
125 |
|
---|
126 | /*!
|
---|
127 | * \class LinearDynamicEquation
|
---|
128 | * \brief This class describes a linear dynamic equation
|
---|
129 | */
|
---|
130 | template <template <class> class S=GaussianSet, class G=Gaussian > class LinearDynamicEquation:public filter::kalman::LinearDynamicEquation<G>{
|
---|
131 | public :
|
---|
132 | /*!
|
---|
133 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
134 | * \param s : weighted kalman state at time k-1
|
---|
135 | */
|
---|
136 | virtual void EvaluateParameters(G *s)=0;
|
---|
137 |
|
---|
138 | /*!
|
---|
139 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
140 | * \param in : the a posteriori set of gaussian at time k-1
|
---|
141 | * \param out : the a priori set of gaussian at time k
|
---|
142 | */
|
---|
143 | virtual void Predict(S<G> *in,S<G> *out);
|
---|
144 |
|
---|
145 | /*!
|
---|
146 | * \brief Destructor
|
---|
147 | */
|
---|
148 | virtual ~LinearDynamicEquation(){}
|
---|
149 | };
|
---|
150 |
|
---|
151 |
|
---|
152 | template <template <class> class S, class G> void LinearDynamicEquation<S,G>::Predict(S<G> *in,S<G> *out){
|
---|
153 | for(size_t i=0;i<in->gaussians.size();i++){
|
---|
154 | EvaluateParameters(&in->gaussians[i]);
|
---|
155 |
|
---|
156 | out->gaussians[i].P= filter::kalman::LinearDynamicEquation<G>::_A*in->gaussians[i].P*Trans( filter::kalman::LinearDynamicEquation<G>::_A)
|
---|
157 | +filter::kalman::LinearDynamicEquation<G>::_B*filter::kalman::LinearDynamicEquation<G>::_Q*Trans(filter::kalman::LinearDynamicEquation<G>::_B);
|
---|
158 | out->gaussians[i].X= filter::kalman::LinearDynamicEquation<G>::_A*in->gaussians[i].X
|
---|
159 | +filter::kalman::LinearDynamicEquation<G>::_B*filter::kalman::LinearDynamicEquation<G>::_U;
|
---|
160 | out->gaussians[i].weight=in->gaussians[i].weight;
|
---|
161 | }
|
---|
162 | }
|
---|
163 |
|
---|
164 |
|
---|
165 | /*!
|
---|
166 | * \class NonLinearDynamicEquation
|
---|
167 | * \brief This class describes a non linear dynamic equation
|
---|
168 | */
|
---|
169 | template <template <class> class S=GaussianSet, class G=Gaussian > class NonLinearDynamicEquation:public filter::kalman::NonLinearDynamicEquation<G>{
|
---|
170 | public :
|
---|
171 | /*!
|
---|
172 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
173 | * \param s : weighted kalman state at time k-1
|
---|
174 | * f= \n
|
---|
175 | * F= \n
|
---|
176 | * G= \n
|
---|
177 | */
|
---|
178 | virtual void EvaluateParameters(G *s)=0;
|
---|
179 |
|
---|
180 | /*!
|
---|
181 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
182 | * \param in : the a posteriori set of gaussian at time k-1
|
---|
183 | * \param out : the a priori set of gaussian at time k
|
---|
184 | */
|
---|
185 | virtual void Predict(S<G> *in,S<G> *out);
|
---|
186 |
|
---|
187 | /*!
|
---|
188 | * \brief Destructor
|
---|
189 | */
|
---|
190 | virtual ~NonLinearDynamicEquation(){}
|
---|
191 | };
|
---|
192 |
|
---|
193 |
|
---|
194 | template <template <class> class S, class G> void NonLinearDynamicEquation<S,G>::Predict(S<G> *in,S<G> *out){
|
---|
195 | for(size_t i=0;i<in->gaussians.size();i++){
|
---|
196 | EvaluateParameters(&in->gaussians[i]);
|
---|
197 |
|
---|
198 | out->gaussians[i].P=filter::kalman::NonLinearDynamicEquation<G>::_F*in->gaussians[i].P*Trans(filter::kalman::NonLinearDynamicEquation<G>::_F)
|
---|
199 | +filter::kalman::NonLinearDynamicEquation<G>::_G*filter::kalman::NonLinearDynamicEquation<G>::_Q*Trans(filter::kalman::NonLinearDynamicEquation<G>::_G);
|
---|
200 | out->gaussians[i].X=filter::kalman::NonLinearDynamicEquation<G>::_f;
|
---|
201 | out->gaussians[i].weight=in->gaussians[i].weight;
|
---|
202 | }
|
---|
203 | }
|
---|
204 |
|
---|
205 | /*!
|
---|
206 | * \class LinearMeasureEquation
|
---|
207 | * \brief This class describes a linear measure equation
|
---|
208 | */
|
---|
209 | template <template <class> class S=GaussianSet, class G=Gau
|
---|
210 | ssian >class LinearMeasureEquation:public filter::kalman::LinearMeasureEquation<G>{
|
---|
211 | public :
|
---|
212 |
|
---|
213 | /*!
|
---|
214 | * \brief virtual method where parameters of the measure equation must be evaluated
|
---|
215 | * \param s : weighted kalman state at time k-1
|
---|
216 | */
|
---|
217 | virtual void EvaluateParameters(G *s)=0;
|
---|
218 |
|
---|
219 |
|
---|
220 | /*!
|
---|
221 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
222 | * \param in : the a priori set of gaussian at time k
|
---|
223 | * \param out : the a posteriori set of gaussian at time k
|
---|
224 | */
|
---|
225 | virtual void Update(S<G> *in,S<G> *out);
|
---|
226 |
|
---|
227 |
|
---|
228 |
|
---|
229 |
|
---|
230 | /*!
|
---|
231 | * \brief Destructor
|
---|
232 | */
|
---|
233 | virtual ~LinearMeasureEquation(){};
|
---|
234 |
|
---|
235 | protected :
|
---|
236 |
|
---|
237 | /*!
|
---|
238 | * \brief virtual method where likelihood value for each particle is computed
|
---|
239 | * \param s : a priori weighted kalman state at time k
|
---|
240 | * \return likelihood value
|
---|
241 | */
|
---|
242 | virtual double ZPDF(G *s);
|
---|
243 |
|
---|
244 | };
|
---|
245 |
|
---|
246 |
|
---|
247 | template <template <class> class S, class G> void LinearMeasureEquation<S,G>::Update(S<G> *in,S<G> *out){
|
---|
248 |
|
---|
249 | for(size_t i=0;i<in->gaussians.size();i++){
|
---|
250 | filter::kalman::LinearMeasureEquation<G>::_coherency=true;
|
---|
251 | EvaluateParameters(&in->gaussians[i]);
|
---|
252 |
|
---|
253 | if(filter::kalman::LinearMeasureEquation<G>::_coherency){
|
---|
254 | filter::kalman::LinearMeasureEquation<G>::_K=in->gaussians[i].P*Trans(filter::kalman::LinearMeasureEquation<G>::_H) *
|
---|
255 | ( InvQR(filter::kalman::LinearMeasureEquation<G>::_H*in->gaussians[i].P*Trans(filter::kalman::LinearMeasureEquation<G>::_H)+filter::kalman::LinearMeasureEquation<G>::_R) );
|
---|
256 | out->gaussians[i].X=in->gaussians[i].X+filter::kalman::LinearMeasureEquation<G>::_K*(filter::kalman::LinearMeasureEquation<G>::_Z-filter::kalman::LinearMeasureEquation<G>::_H*in->gaussians[i].X);
|
---|
257 | out->gaussians[i].P=in->gaussians[i].P-filter::kalman::LinearMeasureEquation<G>::_K*filter::kalman::LinearMeasureEquation<G>::_H*in->gaussians[i].P;
|
---|
258 | }
|
---|
259 |
|
---|
260 | out->gaussians[i].weigh=ZPDF(&in->gaussian[i])*in->gaussians[i].weight;
|
---|
261 |
|
---|
262 | }
|
---|
263 |
|
---|
264 | //out->NormalizeWeights();
|
---|
265 | }
|
---|
266 |
|
---|
267 | template <template <class> class S, class G> double LinearMeasureEquation<S,G>::ZPDF(G *s){
|
---|
268 | Vector apvec=filter::kalman::LinearMeasureEquation<G>::_H*s->X;
|
---|
269 | Matrix apcov=filter::kalman::LinearMeasureEquation<G>::_H*s->P*Trans(filter::kalman::LinearMeasureEquation<G>::_H)+filter::kalman::LinearMeasureEquation<G>::_R;
|
---|
270 | Matrix apinv=InvQR(apcov);
|
---|
271 | double apdet=Det(apcov);
|
---|
272 | return (std::exp(-0.5*Dot(filter::kalman::LinearMeasureEquation<G>::_Z-apvec,apinv*(filter::kalman::LinearMeasureEquation<G>::_Z-apvec)))/
|
---|
273 | (std::sqrt(std::pow(2*M_PI,static_cast<int>(apvec.size()))*std::abs(apdet))));
|
---|
274 | }
|
---|
275 |
|
---|
276 |
|
---|
277 | /*!
|
---|
278 | * \class NonLinearMeasureEquation
|
---|
279 | * \brief This class describes a non linear measure equation
|
---|
280 | */
|
---|
281 | template <template <class> class S=GaussianSet, class G=Gaussian > class NonLinearMeasureEquation:public filter::kalman::NonLinearMeasureEquation<G>{
|
---|
282 | public :
|
---|
283 | /*!
|
---|
284 | * \brief virtual method where parameters of the measure equation must be evaluated
|
---|
285 | * \param s : weighted kalman state at time k-1
|
---|
286 | * h= \n
|
---|
287 | * H= \n
|
---|
288 | */
|
---|
289 | virtual void EvaluateParameters(G *s )=0;
|
---|
290 |
|
---|
291 | /*!
|
---|
292 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
293 | * \param in : the a priori set of gaussian at time k
|
---|
294 | * \param out : the a posteriori set of gaussian at time k
|
---|
295 | */
|
---|
296 | virtual void Update(S<G> *in,S<G> *out);
|
---|
297 |
|
---|
298 | /*!
|
---|
299 | * \brief destructor
|
---|
300 | */
|
---|
301 | virtual ~NonLinearMeasureEquation(){}
|
---|
302 |
|
---|
303 | protected :
|
---|
304 |
|
---|
305 | /*!
|
---|
306 | * \brief virtual method where likelihood value for each particle is computed
|
---|
307 | * \param s : a priori weighted kalman state at time k
|
---|
308 | * \return likelihood value
|
---|
309 | */
|
---|
310 | virtual double ZPDF(G *s);
|
---|
311 |
|
---|
312 | };
|
---|
313 |
|
---|
314 | template <template <class> class S, class G> void NonLinearMeasureEquation<S,G>::Update(S<G> *in,S<G> *out){
|
---|
315 |
|
---|
316 | for(size_t i=0;i<in->gaussians.size();i++){
|
---|
317 | filter::kalman::NonLinearMeasureEquation<G>::_coherency=true;
|
---|
318 | EvaluateParameters(&in->gaussians[i]);
|
---|
319 |
|
---|
320 | if(filter::kalman::NonLinearMeasureEquation<G>::_coherency){
|
---|
321 | filter::kalman::NonLinearMeasureEquation<G>::_K=in->gaussians[i].P*Trans(filter::kalman::NonLinearMeasureEquation<G>::_H)*(InvQR(filter::kalman::NonLinearMeasureEquation<G>::_H*in->gaussians[i].P*Trans(filter::kalman::NonLinearMeasureEquation<G>::_H) + filter::kalman::NonLinearMeasureEquation<G>::_R));
|
---|
322 | out->gaussians[i].X=in->gaussians[i].X+filter::kalman::NonLinearMeasureEquation<G>::_K*(filter::kalman::NonLinearMeasureEquation<G>::_Z-filter::kalman::NonLinearMeasureEquation<G>::_h);
|
---|
323 | out->gaussians[i].P=in->gaussians[i].P-filter::kalman::NonLinearMeasureEquation<G>::_K*filter::kalman::NonLinearMeasureEquation<G>::_H*in->gaussians[i].P;
|
---|
324 | }
|
---|
325 | out->gaussians[i].weight=ZPDF(&in->gaussians[i])*in->gaussians[i].weight;
|
---|
326 | }
|
---|
327 |
|
---|
328 | //out->NormalizeWeights();
|
---|
329 | }
|
---|
330 |
|
---|
331 | template <template <class> class S, class G> double NonLinearMeasureEquation<S,G>::ZPDF(G *s){
|
---|
332 | Vector apvec=filter::kalman::NonLinearMeasureEquation<G>::_H*s->X;
|
---|
333 | Matrix apcov=filter::kalman::NonLinearMeasureEquation<G>::_H*s->P*Trans(filter::kalman::NonLinearMeasureEquation<G>::_H)+filter::kalman::NonLinearMeasureEquation<G>::_R;
|
---|
334 | Matrix apinv=InvQR(apcov);
|
---|
335 | double apdet=Det(apcov);
|
---|
336 | return (std::exp(-0.5*Dot(filter::kalman::NonLinearMeasureEquation<G>::_Z-apvec,apinv*(filter::kalman::NonLinearMeasureEquation<G>::_Z-apvec)))/
|
---|
337 | (std::sqrt(std::pow(2*M_PI,static_cast<int>(apvec.size()))*std::abs(apdet))));
|
---|
338 | }
|
---|
339 |
|
---|
340 | } // namespace gaussiansum
|
---|
341 | } // namespace filter
|
---|
342 |
|
---|
343 | #endif // __GAUSSIAN_FILTERING_BASE__
|
---|