source: pacpusframework/trunk/include/Pacpus/PacpusTools/filtering/gaussian_sum_filtering.hpp@ 73

Last change on this file since 73 was 73, checked in by Marek Kurdej, 11 years ago

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