source: pacpusframework/branches/2.0-beta1/include/Pacpus/PacpusTools/filtering/gaussian_sum_filtering.hpp@ 89

Last change on this file since 89 was 89, checked in by morasjul, 11 years ago

PACPUS 2.0 Beta deployed in new branch

Major changes:
-Add communication interface between components
-Add examples for communications interface (TestComponents)
-Move to Qt5 support

  • Property svn:executable set to *
File size: 12.9 KB
Line 
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
22namespace filter{
23
24namespace 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
210ssian >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__
Note: See TracBrowser for help on using the repository browser.