[89] | 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__
|
---|