[8] | 1 | /*! |
---|
| 2 | \file |
---|
| 3 | \brief Bayesian Filtering using stochastic sampling (Particle Filters) |
---|
| 4 | \author Vaclav Smidl. |
---|
| 5 | |
---|
| 6 | ----------------------------------- |
---|
| 7 | BDM++ - C++ library for Bayesian Decision Making under Uncertainty |
---|
| 8 | |
---|
| 9 | Using IT++ for numerical operations |
---|
| 10 | ----------------------------------- |
---|
| 11 | */ |
---|
| 12 | |
---|
[384] | 13 | #ifndef PARTICLES_H |
---|
| 14 | #define PARTICLES_H |
---|
[8] | 15 | |
---|
[262] | 16 | |
---|
[384] | 17 | #include "../stat/exp_family.h" |
---|
[8] | 18 | |
---|
[270] | 19 | namespace bdm { |
---|
[8] | 20 | |
---|
| 21 | /*! |
---|
[32] | 22 | * \brief Trivial particle filter with proposal density equal to parameter evolution model. |
---|
[8] | 23 | |
---|
[32] | 24 | Posterior density is represented by a weighted empirical density (\c eEmp ). |
---|
[8] | 25 | */ |
---|
[32] | 26 | |
---|
| 27 | class PF : public BM { |
---|
[8] | 28 | protected: |
---|
[32] | 29 | //!number of particles; |
---|
| 30 | int n; |
---|
| 31 | //!posterior density |
---|
| 32 | eEmp est; |
---|
| 33 | //! pointer into \c eEmp |
---|
| 34 | vec &_w; |
---|
| 35 | //! pointer into \c eEmp |
---|
| 36 | Array<vec> &_samples; |
---|
| 37 | //! Parameter evolution model |
---|
[270] | 38 | mpdf *par; |
---|
[32] | 39 | //! Observation model |
---|
[270] | 40 | mpdf *obs; |
---|
[281] | 41 | |
---|
[283] | 42 | //! which resampling method will be used |
---|
| 43 | RESAMPLING_METHOD resmethod; |
---|
| 44 | |
---|
[281] | 45 | //! \name Options |
---|
| 46 | //!@{ |
---|
| 47 | |
---|
| 48 | //! Log all samples |
---|
| 49 | bool opt_L_smp; |
---|
[283] | 50 | //! Log all samples |
---|
| 51 | bool opt_L_wei; |
---|
[281] | 52 | //!@} |
---|
| 53 | |
---|
[8] | 54 | public: |
---|
[270] | 55 | //! \name Constructors |
---|
| 56 | //!@{ |
---|
[477] | 57 | PF ( ) : est(), _w ( est._w() ), _samples ( est._samples() ), opt_L_smp ( false ), opt_L_wei ( false ) { |
---|
| 58 | LIDs.set_size ( 5 ); |
---|
| 59 | }; |
---|
[283] | 60 | /* PF ( mpdf *par0, mpdf *obs0, epdf *epdf0, int n0 ) : |
---|
| 61 | est ( ),_w ( est._w() ),_samples ( est._samples() ),opt_L_smp(false), opt_L_wei(false) |
---|
| 62 | { set_parameters ( par0,obs0,n0 ); set_statistics ( ones ( n0 ),epdf0 ); };*/ |
---|
[477] | 63 | void set_parameters ( mpdf *par0, mpdf *obs0, int n0, RESAMPLING_METHOD rm = SYSTEMATIC ) { |
---|
| 64 | par = par0; |
---|
| 65 | obs = obs0; |
---|
| 66 | n = n0; |
---|
| 67 | resmethod = rm; |
---|
| 68 | }; |
---|
| 69 | void set_statistics ( const vec w0, epdf *epdf0 ) { |
---|
| 70 | est.set_statistics ( w0, epdf0 ); |
---|
| 71 | }; |
---|
[270] | 72 | //!@} |
---|
[33] | 73 | //! Set posterior density by sampling from epdf0 |
---|
[283] | 74 | // void set_est ( const epdf &epdf0 ); |
---|
| 75 | void set_options ( const string &opt ) { |
---|
[477] | 76 | BM::set_options ( opt ); |
---|
| 77 | opt_L_wei = ( opt.find ( "logweights" ) != string::npos ); |
---|
| 78 | opt_L_smp = ( opt.find ( "logsamples" ) != string::npos ); |
---|
[281] | 79 | } |
---|
[32] | 80 | void bayes ( const vec &dt ); |
---|
[225] | 81 | //!access function |
---|
[477] | 82 | vec* __w() { |
---|
| 83 | return &_w; |
---|
| 84 | } |
---|
[8] | 85 | }; |
---|
| 86 | |
---|
| 87 | /*! |
---|
[32] | 88 | \brief Marginalized Particle filter |
---|
[8] | 89 | |
---|
[98] | 90 | Trivial version: proposal = parameter evolution, observation model is not used. (it is assumed to be part of BM). |
---|
[8] | 91 | */ |
---|
| 92 | |
---|
[32] | 93 | template<class BM_T> |
---|
| 94 | class MPF : public PF { |
---|
[283] | 95 | Array<BM_T*> BMs; |
---|
[32] | 96 | |
---|
| 97 | //! internal class for MPDF providing composition of eEmp with external components |
---|
| 98 | |
---|
[477] | 99 | class mpfepdf : public epdf { |
---|
[32] | 100 | protected: |
---|
| 101 | eEmp &E; |
---|
| 102 | vec &_w; |
---|
[170] | 103 | Array<const epdf*> Coms; |
---|
[8] | 104 | public: |
---|
[281] | 105 | mpfepdf ( eEmp &E0 ) : |
---|
[270] | 106 | epdf ( ), E ( E0 ), _w ( E._w() ), |
---|
[32] | 107 | Coms ( _w.length() ) { |
---|
| 108 | }; |
---|
[283] | 109 | //! read statistics from MPF |
---|
| 110 | void read_statistics ( Array<BM_T*> &A ) { |
---|
[477] | 111 | dim = E.dimension() + A ( 0 )->posterior().dimension(); |
---|
| 112 | for ( int i = 0; i < _w.length() ; i++ ) { |
---|
| 113 | Coms ( i ) = A ( i )->_e(); |
---|
| 114 | } |
---|
[283] | 115 | } |
---|
| 116 | //! needed in resampling |
---|
[477] | 117 | void set_elements ( int &i, double wi, const epdf* ep ) { |
---|
| 118 | _w ( i ) = wi; |
---|
| 119 | Coms ( i ) = ep; |
---|
| 120 | }; |
---|
[32] | 121 | |
---|
[283] | 122 | void set_parameters ( int n ) { |
---|
| 123 | E.set_parameters ( n, false ); |
---|
| 124 | Coms.set_length ( n ); |
---|
| 125 | } |
---|
[32] | 126 | vec mean() const { |
---|
| 127 | // ugly |
---|
[477] | 128 | vec pom = zeros ( Coms ( 0 )->dimension() ); |
---|
| 129 | for ( int i = 0; i < _w.length(); i++ ) { |
---|
| 130 | pom += Coms ( i )->mean() * _w ( i ); |
---|
| 131 | } |
---|
| 132 | return concat ( E.mean(), pom ); |
---|
[32] | 133 | } |
---|
[229] | 134 | vec variance() const { |
---|
| 135 | // ugly |
---|
[477] | 136 | vec pom = zeros ( Coms ( 0 )->dimension() ); |
---|
| 137 | vec pom2 = zeros ( Coms ( 0 )->dimension() ); |
---|
| 138 | for ( int i = 0; i < _w.length(); i++ ) { |
---|
[229] | 139 | pom += Coms ( i )->mean() * _w ( i ); |
---|
[477] | 140 | pom2 += ( Coms ( i )->variance() + pow ( Coms ( i )->mean(), 2 ) ) * _w ( i ); |
---|
[270] | 141 | } |
---|
[477] | 142 | return concat ( E.variance(), pom2 - pow ( pom, 2 ) ); |
---|
[229] | 143 | } |
---|
[477] | 144 | void qbounds ( vec &lb, vec &ub, double perc = 0.95 ) const { |
---|
[283] | 145 | //bounds on particles |
---|
| 146 | vec lbp; |
---|
| 147 | vec ubp; |
---|
[477] | 148 | E.qbounds ( lbp, ubp ); |
---|
[32] | 149 | |
---|
[283] | 150 | //bounds on Components |
---|
[477] | 151 | int dimC = Coms ( 0 )->dimension(); |
---|
[283] | 152 | int j; |
---|
| 153 | // temporary |
---|
[477] | 154 | vec lbc ( dimC ); |
---|
| 155 | vec ubc ( dimC ); |
---|
[283] | 156 | // minima and maxima |
---|
[477] | 157 | vec Lbc ( dimC ); |
---|
| 158 | vec Ubc ( dimC ); |
---|
[283] | 159 | Lbc = std::numeric_limits<double>::infinity(); |
---|
| 160 | Ubc = -std::numeric_limits<double>::infinity(); |
---|
| 161 | |
---|
[477] | 162 | for ( int i = 0; i < _w.length(); i++ ) { |
---|
[283] | 163 | // check Coms |
---|
[477] | 164 | Coms ( i )->qbounds ( lbc, ubc ); |
---|
| 165 | for ( j = 0; j < dimC; j++ ) { |
---|
| 166 | if ( lbc ( j ) < Lbc ( j ) ) { |
---|
| 167 | Lbc ( j ) = lbc ( j ); |
---|
| 168 | } |
---|
| 169 | if ( ubc ( j ) > Ubc ( j ) ) { |
---|
| 170 | Ubc ( j ) = ubc ( j ); |
---|
| 171 | } |
---|
[283] | 172 | } |
---|
| 173 | } |
---|
[477] | 174 | lb = concat ( lbp, Lbc ); |
---|
| 175 | ub = concat ( ubp, Ubc ); |
---|
[283] | 176 | } |
---|
| 177 | |
---|
[477] | 178 | vec sample() const { |
---|
| 179 | it_error ( "Not implemented" ); |
---|
| 180 | return 0; |
---|
| 181 | } |
---|
[32] | 182 | |
---|
[477] | 183 | double evallog ( const vec &val ) const { |
---|
| 184 | it_error ( "not implemented" ); |
---|
| 185 | return 0.0; |
---|
| 186 | } |
---|
[32] | 187 | }; |
---|
| 188 | |
---|
[281] | 189 | //! Density joining PF.est with conditional parts |
---|
[32] | 190 | mpfepdf jest; |
---|
| 191 | |
---|
[281] | 192 | //! Log means of BMs |
---|
| 193 | bool opt_L_mea; |
---|
[283] | 194 | |
---|
[32] | 195 | public: |
---|
| 196 | //! Default constructor. |
---|
[283] | 197 | MPF () : PF (), jest ( est ) {}; |
---|
[477] | 198 | void set_parameters ( mpdf *par0, mpdf *obs0, int n0, RESAMPLING_METHOD rm = SYSTEMATIC ) { |
---|
[283] | 199 | PF::set_parameters ( par0, obs0, n0, rm ); |
---|
| 200 | jest.set_parameters ( n0 );//duplication of rm |
---|
| 201 | BMs.set_length ( n0 ); |
---|
| 202 | } |
---|
| 203 | void set_statistics ( epdf *epdf0, const BM_T* BMcond0 ) { |
---|
[32] | 204 | |
---|
[477] | 205 | PF::set_statistics ( ones ( n ) / n, epdf0 ); |
---|
[283] | 206 | // copy |
---|
[477] | 207 | for ( int i = 0; i < n; i++ ) { |
---|
| 208 | BMs ( i ) = new BM_T ( *BMcond0 ); |
---|
| 209 | BMs ( i )->condition ( _samples ( i ) ); |
---|
| 210 | } |
---|
[32] | 211 | |
---|
[283] | 212 | jest.read_statistics ( BMs ); |
---|
| 213 | //options |
---|
[32] | 214 | }; |
---|
| 215 | |
---|
| 216 | void bayes ( const vec &dt ); |
---|
[477] | 217 | const epdf& posterior() const { |
---|
| 218 | return jest; |
---|
| 219 | } |
---|
| 220 | const epdf* _e() const { |
---|
| 221 | return &jest; //Fixme: is it useful? |
---|
| 222 | } |
---|
[283] | 223 | //! Set postrior of \c rvc to samples from epdf0. Statistics of BMs are not re-computed! Use only for initialization! |
---|
| 224 | /* void set_est ( const epdf& epdf0 ) { |
---|
| 225 | PF::set_est ( epdf0 ); // sample params in condition |
---|
| 226 | // copy conditions to BMs |
---|
[32] | 227 | |
---|
[283] | 228 | for ( int i=0;i<n;i++ ) {BMs(i)->condition ( _samples ( i ) );} |
---|
| 229 | }*/ |
---|
| 230 | void set_options ( const string &opt ) { |
---|
| 231 | PF::set_options ( opt ); |
---|
[477] | 232 | opt_L_mea = ( opt.find ( "logmeans" ) != string::npos ); |
---|
[32] | 233 | } |
---|
[283] | 234 | |
---|
[225] | 235 | //!Access function |
---|
[477] | 236 | BM* _BM ( int i ) { |
---|
| 237 | return BMs ( i ); |
---|
| 238 | } |
---|
[8] | 239 | }; |
---|
| 240 | |
---|
[32] | 241 | template<class BM_T> |
---|
| 242 | void MPF<BM_T>::bayes ( const vec &dt ) { |
---|
| 243 | int i; |
---|
| 244 | vec lls ( n ); |
---|
[60] | 245 | vec llsP ( n ); |
---|
[32] | 246 | ivec ind; |
---|
[477] | 247 | double mlls = -std::numeric_limits<double>::infinity(); |
---|
[32] | 248 | |
---|
[270] | 249 | #pragma omp parallel for |
---|
[477] | 250 | for ( i = 0; i < n; i++ ) { |
---|
[32] | 251 | //generate new samples from paramater evolution model; |
---|
[487] | 252 | vec old_smp=_samples ( i ); |
---|
| 253 | _samples ( i ) = par->samplecond ( old_smp ); |
---|
| 254 | llsP ( i ) = par->evallogcond ( _samples ( i ), old_smp ); |
---|
[283] | 255 | BMs ( i )->condition ( _samples ( i ) ); |
---|
| 256 | BMs ( i )->bayes ( dt ); |
---|
| 257 | lls ( i ) = BMs ( i )->_ll(); // lls above is also in proposal her must be lls(i) =, not +=!! |
---|
[477] | 258 | if ( lls ( i ) > mlls ) mlls = lls ( i ); //find maximum likelihood (for numerical stability) |
---|
[32] | 259 | } |
---|
[115] | 260 | |
---|
[477] | 261 | double sum_w = 0.0; |
---|
[32] | 262 | // compute weights |
---|
[270] | 263 | #pragma omp parallel for |
---|
[477] | 264 | for ( i = 0; i < n; i++ ) { |
---|
[32] | 265 | _w ( i ) *= exp ( lls ( i ) - mlls ); // multiply w by likelihood |
---|
[477] | 266 | sum_w += _w ( i ); |
---|
[32] | 267 | } |
---|
| 268 | |
---|
[477] | 269 | if ( sum_w > 0.0 ) { |
---|
| 270 | _w /= sum_w; //? |
---|
| 271 | } else { |
---|
| 272 | cout << "sum(w)==0" << endl; |
---|
[270] | 273 | } |
---|
[32] | 274 | |
---|
[60] | 275 | |
---|
[477] | 276 | double eff = 1.0 / ( _w * _w ); |
---|
[67] | 277 | if ( eff < ( 0.3*n ) ) { |
---|
[283] | 278 | ind = est.resample ( resmethod ); |
---|
[32] | 279 | // Resample Bms! |
---|
| 280 | |
---|
[270] | 281 | #pragma omp parallel for |
---|
[477] | 282 | for ( i = 0; i < n; i++ ) { |
---|
| 283 | if ( ind ( i ) != i ) {//replace the current Bm by a new one |
---|
[32] | 284 | //fixme this would require new assignment operator |
---|
| 285 | // *Bms[i] = *Bms[ind ( i ) ]; |
---|
[60] | 286 | |
---|
[32] | 287 | // poor-man's solution: replicate constructor here |
---|
| 288 | // copied from MPF::MPF |
---|
[283] | 289 | delete BMs ( i ); |
---|
| 290 | BMs ( i ) = new BM_T ( *BMs ( ind ( i ) ) ); //copy constructor |
---|
[477] | 291 | const epdf& pom = BMs ( i )->posterior(); |
---|
| 292 | jest.set_elements ( i, 1.0 / n, &pom ); |
---|
[32] | 293 | } |
---|
| 294 | }; |
---|
[60] | 295 | cout << '.'; |
---|
[32] | 296 | } |
---|
| 297 | } |
---|
| 298 | |
---|
[254] | 299 | } |
---|
[8] | 300 | #endif // KF_H |
---|
| 301 | |
---|