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

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

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