pf
rbpf.h
Go to the documentation of this file.
1 #ifndef RBPF_H
2 #define RBPF_H
3 
4 #include <functional>
5 #include <vector>
6 #include <array>
7 #include <Eigen/Dense>
8 #include <algorithm> // std::fill
9 
10 #include "pf_base.h"
11 #include "cf_filters.h" // for closed form filter objects
12 
13 
15 
26 template<size_t nparts, size_t dimnss,size_t dimss,size_t dimy,typename resamp_t, typename float_t >
27 class rbpf_hmm : public rbpf_base<float_t, dimss, dimnss, dimy >
28 {
29 public:
30 
32  using sssv = Eigen::Matrix<float_t,dimss,1>;
34  using nsssv = Eigen::Matrix<float_t,dimnss,1>;
36  using osv = Eigen::Matrix<float_t,dimy,1>;
38  using nsssMat = Eigen::Matrix<float_t,dimnss,dimnss>;
40  using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
42  using arrayMod = std::array<hmm<dimnss,dimy,float_t>,nparts>;
44  using arrayVec = std::array<sssv,nparts>;
46  using arrayfloat_t = std::array<float_t,nparts>;
47 
48 
50 
54  rbpf_hmm(const unsigned int &resamp_sched=1);
55 
56 
60  virtual ~rbpf_hmm();
61 
62 
64 
69  void filter(const osv &data,
70  const std::vector<std::function<const Mat(const nsssv &x1tProbs, const sssv &x2t)> >& fs
71  = std::vector<std::function<const Mat(const nsssv&, const sssv&)> >());//, const std::vector<std::function<const Mat(const Vec&)> >& fs);
72 
73 
75 
79  float_t getLogCondLike() const;
80 
82 
86  std::vector<Mat> getExpectations() const;
87 
89 
94  virtual float_t logMuEv(const sssv &x21) = 0;
95 
96 
98 
103  virtual sssv q1Samp(const osv &y1) = 0;
104 
105 
107 
112  virtual nsssv initHMMProbVec(const sssv &x21) = 0;
113 
114 
116 
121  virtual nsssMat initHMMTransMat(const sssv &x21) = 0;
122 
124 
130  virtual sssv qSamp(const sssv &x2tm1, const osv &yt) = 0;
131 
132 
134 
140  virtual float_t logQ1Ev(const sssv &x21, const osv &y1) = 0;
141 
142 
144 
150  virtual float_t logFEv(const sssv &x2t, const sssv &x2tm1) = 0;
151 
152 
154 
161  virtual float_t logQEv(const sssv &x2t, const sssv &x2tm1, const osv &yt ) = 0;
162 
163 
165 
171  virtual void updateHMM(hmm<dimnss,dimy,float_t> &aModel, const osv &yt, const sssv &x2t) = 0;
172 
173 private:
174 
176  unsigned int m_now;
180  unsigned int m_rs;
188  resamp_t m_resampler;
190  std::vector<Mat> m_expectations;
191 
192 };
193 
194 
195 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
197  : m_now(0)
198  , m_lastLogCondLike(0.0)
199  , m_rs(resamp_sched)
200 {
201  std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
202 }
203 
204 
205 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
207 
208 
209 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
210 void rbpf_hmm<nparts,dimnss,dimss,dimy,resamp_t,float_t>::filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1tProbs, const sssv &x2t)> >& fs)
211 {
212 
213  if(m_now > 0)
214  { //m_now > 0
215 
216  // update
217  sssv newX2Samp;
218  float_t sumexpdenom(0.0);
219  float_t m1(-std::numeric_limits<float_t>::infinity()); // for revised log weights
220  float_t m2 = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
221  for(size_t ii = 0; ii < nparts; ++ii){
222 
223  newX2Samp = qSamp(m_p_samps[ii], data);
224  updateHMM(m_p_innerMods[ii], data, newX2Samp);
225  sumexpdenom += std::exp(m_logUnNormWeights[ii] - m2);
226 
227  m_logUnNormWeights[ii] += m_p_innerMods[ii].getLogCondLike()
228  + logFEv(newX2Samp, m_p_samps[ii])
229  - logQEv(newX2Samp, m_p_samps[ii], data);
230 
231  // update a max
232  if(m_logUnNormWeights[ii] > m1)
233  m1 = m_logUnNormWeights[ii];
234 
235  m_p_samps[ii] = newX2Samp;
236  }
237 
238  // calculate log p(y_t | y_{1:t-1})
239  float_t sumexpnumer(0.0);
240  for(size_t p = 0; p < nparts; ++p)
241  sumexpnumer += std::exp(m_logUnNormWeights[p] - m1);
242  m_lastLogCondLike = m1 + std::log(sumexpnumer) - m2 - std::log(sumexpdenom);
243 
244  // calculate expectations before you resample
245  unsigned int fId(0);
246  //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
247  for(auto & h : fs){
248 
249  Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
250  unsigned int rows = testOutput.rows();
251  unsigned int cols = testOutput.cols();
252  Mat numer = Mat::Zero(rows,cols);
253  float_t denom(0.0);
254  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
255  numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
256  denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
257  }
258  m_expectations[fId] = numer/denom;
259  fId++;
260  }
261 
262  // resample (unnormalized weights ok)
263  if( (m_now+1) % m_rs == 0)
265 
266  // update time step
267  m_now ++;
268  }
269  else //( m_now == 0)
270  { // first data point coming
271 
272  // initialize and update the closed-form mods
273  nsssv tmpProbs;
274  nsssMat tmpTransMat;
275  float_t m1(-std::numeric_limits<float_t>::infinity());
276  for(size_t ii = 0; ii < nparts; ++ii){
277 
278  m_p_samps[ii] = q1Samp(data);
279  tmpProbs = initHMMProbVec(m_p_samps[ii]);
280  tmpTransMat = initHMMTransMat(m_p_samps[ii]);
281  m_p_innerMods[ii] = hmm<dimnss,dimy,float_t>(tmpProbs, tmpTransMat);
282  this->updateHMM(m_p_innerMods[ii], data, m_p_samps[ii]);
283  m_logUnNormWeights[ii] = m_p_innerMods[ii].getLogCondLike() + logMuEv(m_p_samps[ii]) - logQ1Ev(m_p_samps[ii], data);
284 
285  // maximum to be used in likelihood calc
286  if(m_logUnNormWeights[ii] > m1)
287  m1 = m_logUnNormWeights[ii];
288  }
289 
290  // calc log p(y1)
291  float_t sumexp(0.0);
292  for(size_t p = 0; p < nparts; ++p)
293  sumexp += std::exp(m_logUnNormWeights[p] - m1);
294  m_lastLogCondLike = m1 + std::log(sumexp) - std::log(static_cast<float_t>(nparts));
295 
296  // calculate expectations before you resample
297  m_expectations.resize(fs.size());
298  unsigned int fId(0);
299  //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
300  for(auto & h : fs){
301 
302  Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
303  unsigned int rows = testOutput.rows();
304  unsigned int cols = testOutput.cols();
305  Mat numer = Mat::Zero(rows,cols);
306  float_t denom(0.0);
307  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
308  numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
309  denom += std::exp(m_logUnNormWeights[prtcl] - m1);
310  }
311  m_expectations[fId] = numer/denom;
312  fId++;
313  }
314 
315  // resample (unnormalized weights ok)
316  if( (m_now+1) % m_rs == 0)
318 
319  // advance time step
320  m_now ++;
321  }
322 
323 }
324 
325 
326 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
328 {
329  return m_lastLogCondLike;
330 }
331 
332 
333 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
335 {
336  return m_expectations;
337 }
338 
339 
341 
352 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
353 class rbpf_hmm_bs : public rbpf_base<float_t,dimss,dimnss,dimy>
354 {
355 public:
356 
358  using sssv = Eigen::Matrix<float_t,dimss,1>;
360  using nsssv = Eigen::Matrix<float_t,dimnss,1>;
362  using osv = Eigen::Matrix<float_t,dimy,1>;
364  using nsssMat = Eigen::Matrix<float_t,dimnss,dimnss>;
366  using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
368  using arrayMod = std::array<hmm<dimnss,dimy,float_t>,nparts>;
370  using arrayVec = std::array<sssv,nparts>;
372  using arrayfloat_t = std::array<float_t,nparts>;
373 
374 
376 
380  rbpf_hmm_bs(const unsigned int &resamp_sched=1);
381 
382 
386  virtual ~rbpf_hmm_bs();
387 
388 
390 
395  void filter(const osv &data,
396  const std::vector<std::function<const Mat(const nsssv &x1tProbs, const sssv &x2t)> >& fs
397  = std::vector<std::function<const Mat(const nsssv&, const sssv&)> >());//, const std::vector<std::function<const Mat(const Vec&)> >& fs);
398 
399 
401 
405  float_t getLogCondLike() const;
406 
408 
412  std::vector<Mat> getExpectations() const;
413 
414 
416 
420  virtual sssv muSamp() = 0;
421 
422 
424 
429  virtual nsssv initHMMProbVec(const sssv &x21) = 0;
430 
431 
433 
438  virtual nsssMat initHMMTransMat(const sssv &x21) = 0;
439 
441 
446  virtual sssv fSamp(const sssv &x2tm1) = 0;
447 
448 
450 
456  virtual void updateHMM(hmm<dimnss,dimy,float_t> &aModel, const osv &yt, const sssv &x2t) = 0;
457 
458 private:
459 
461  unsigned int m_now;
465  unsigned int m_rs;
473  resamp_t m_resampler;
475  std::vector<Mat> m_expectations;
476 
477 };
478 
479 
480 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
482  : m_now(0)
483  , m_lastLogCondLike(0.0)
484  , m_rs(resamp_sched)
485 {
486  std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
487 }
488 
489 
490 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
492 
493 
494 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
495 void rbpf_hmm_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t>::filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1tProbs, const sssv &x2t)> >& fs)
496 {
497 
498  if(m_now > 0)
499  {
500  // update
501  sssv newX2Samp;
502  float_t sumexpdenom(0.0);
503  float_t m1(-std::numeric_limits<float_t>::infinity()); // for revised log weights
504  float_t m2 = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
505  for(size_t ii = 0; ii < nparts; ++ii){
506 
507  newX2Samp = fSamp(m_p_samps[ii]);
508  updateHMM(m_p_innerMods[ii], data, newX2Samp);
509  sumexpdenom += std::exp(m_logUnNormWeights[ii] - m2);
510 
511  m_logUnNormWeights[ii] += m_p_innerMods[ii].getLogCondLike();
512 
513  // update a max
514  if(m_logUnNormWeights[ii] > m1)
515  m1 = m_logUnNormWeights[ii];
516 
517  m_p_samps[ii] = newX2Samp;
518  }
519 
520  // calculate log p(y_t | y_{1:t-1})
521  float_t sumexpnumer(0.0);
522  for(size_t p = 0; p < nparts; ++p)
523  sumexpnumer += std::exp(m_logUnNormWeights[p] - m1);
524  m_lastLogCondLike = m1 + std::log(sumexpnumer) - m2 - std::log(sumexpdenom);
525 
526  // calculate expectations before you resample
527  unsigned int fId(0);
528  //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
529  for(auto & h : fs){
530 
531  Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
532  unsigned int rows = testOutput.rows();
533  unsigned int cols = testOutput.cols();
534  Mat numer = Mat::Zero(rows,cols);
535  float_t denom(0.0);
536  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
537  numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl])*std::exp(m_logUnNormWeights[prtcl] - m1);
538  denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
539  }
540  m_expectations[fId] = numer/denom;
541  fId++;
542  }
543 
544  // resample (unnormalized weights ok)
545  if( (m_now+1) % m_rs == 0)
547 
548  // update time step
549  m_now ++;
550  }
551  else// ( m_now == 0) // first data point coming
552  {
553  // initialize and update the closed-form mods
554  nsssv tmpProbs;
555  nsssMat tmpTransMat;
556  float_t m1(-std::numeric_limits<float_t>::infinity());
557  for(size_t ii = 0; ii < nparts; ++ii){
558 
559  m_p_samps[ii] = muSamp();
560  tmpProbs = initHMMProbVec(m_p_samps[ii]);
561  tmpTransMat = initHMMTransMat(m_p_samps[ii]);
562  m_p_innerMods[ii] = hmm<dimnss,dimy,float_t>(tmpProbs, tmpTransMat);
563  this->updateHMM(m_p_innerMods[ii], data, m_p_samps[ii]);
564  m_logUnNormWeights[ii] = m_p_innerMods[ii].getLogCondLike();
565 
566  // maximum to be used in likelihood calc
567  if(m_logUnNormWeights[ii] > m1)
568  m1 = m_logUnNormWeights[ii];
569  }
570 
571  // calc log p(y1)
572  float_t sumexp(0.0);
573  for(size_t p = 0; p < nparts; ++p)
574  sumexp += std::exp(m_logUnNormWeights[p] - m1);
575  m_lastLogCondLike = m1 + std::log(sumexp) - std::log(static_cast<float_t>(nparts));
576 
577  // calculate expectations before you resample
578  m_expectations.resize(fs.size());
579  unsigned int fId(0);
580  //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end()); /// TODO: can we just use m1?
581  for(auto & h : fs){
582 
583  Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
584  unsigned int rows = testOutput.rows();
585  unsigned int cols = testOutput.cols();
586  Mat numer = Mat::Zero(rows,cols);
587  float_t denom(0.0);
588  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
589  numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
590  denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
591  }
592  m_expectations[fId] = numer/denom;
593  fId++;
594  }
595 
596  // resample (unnormalized weights ok)
597  if( (m_now+1) % m_rs == 0)
599 
600  // advance time step
601  m_now ++;
602  }
603 
604 }
605 
606 
607 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
609 {
610  return m_lastLogCondLike;
611 }
612 
613 
614 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
616 {
617  return m_expectations;
618 }
619 
620 
622 
633 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
634 class rbpf_kalman : public rbpf_base<float_t,dimss,dimnss,dimy>
635 {
636 
637 public:
638 
640  using sssv = Eigen::Matrix<float_t,dimss,1>;
642  using nsssv = Eigen::Matrix<float_t,dimnss,1>;
644  using osv = Eigen::Matrix<float_t,dimy,1>;
646  using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
648  using nsssMat = Eigen::Matrix<float_t,dimnss,dimnss>;
650  using arrayMod = std::array<kalman<dimnss,dimy,0,float_t>,nparts>;
652  using arrayVec = std::array<sssv,nparts>;
654  using arrayfloat_t = std::array<float_t,nparts>;
655 
657 
660  rbpf_kalman(const unsigned int &resamp_sched=1);
661 
662 
666  virtual ~rbpf_kalman();
667 
668 
670 
675  void filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >& fs
676  = std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >() );
677 
679 
682  float_t getLogCondLike() const;
683 
684 
686 
690  std::vector<Mat> getExpectations() const;
691 
692 
694 
699  virtual float_t logMuEv(const sssv &x21) = 0;
700 
701 
703 
708  virtual sssv q1Samp(const osv &y1) = 0;
709 
710 
712 
717  virtual nsssv initKalmanMean(const sssv &x21) = 0;
718 
719 
721 
726  virtual nsssMat initKalmanVar(const sssv &x21) = 0;
727 
728 
730 
736  virtual sssv qSamp(const sssv &x2tm1, const osv &yt) = 0;
737 
738 
740 
746  virtual float_t logQ1Ev(const sssv &x21, const osv &y1) = 0;
747 
748 
750 
756  virtual float_t logFEv(const sssv &x2t, const sssv &x2tm1) = 0;
757 
758 
760 
767  virtual float_t logQEv(const sssv &x2t, const sssv &x2tm1, const osv &yt) = 0;
768 
769 
771 
777  virtual void updateKalman(kalman<dimnss,dimy,0,float_t> &kMod, const osv &yt, const sssv &x2t) = 0;
778 
779 private:
780 
782  unsigned int m_rs;
790  unsigned int m_now;
794  resamp_t m_resampler;
796  std::vector<Mat> m_expectations;
797 };
798 
799 
800 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
802  : m_now(0)
803  , m_lastLogCondLike(0.0)
804  , m_rs(resamp_sched)
805 {
806  std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
807 }
808 
809 
810 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
812 
813 
814 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
815 void rbpf_kalman<nparts,dimnss,dimss,dimy,resamp_t,float_t>::filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >& fs)
816 {
817 
818  if(m_now > 0)
819  {
820 
821  // update
822  sssv newX2Samp;
823  float_t m1(-std::numeric_limits<float_t>::infinity()); // for updated weights
824  float_t m2 = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
825  float_t sumexpdenom(0.0);
826  for(size_t ii = 0; ii < nparts; ++ii){
827  newX2Samp = qSamp(m_p_samps[ii], data);
828  this->updateKalman(m_p_innerMods[ii], data, newX2Samp);
829 
830  // before you update the weights
831  sumexpdenom += std::exp(m_logUnNormWeights[ii] - m2);
832 
833  // update the weights
834  m_logUnNormWeights[ii] += m_p_innerMods[ii].getLogCondLike() + logFEv(newX2Samp, m_p_samps[ii]) - logQEv(newX2Samp, m_p_samps[ii], data);
835 
836  // update a max
837  if(m_logUnNormWeights[ii] > m1)
838  m1 = m_logUnNormWeights[ii];
839 
840  m_p_samps[ii] = newX2Samp;
841  }
842 
843  // calc log p(y_t | y_{1:t-1})
844  float_t sumexpnumer(0.0);
845  for(size_t p = 0; p < nparts; ++p)
846  sumexpnumer += std::exp(m_logUnNormWeights[p] - m1);
847  m_lastLogCondLike = m1 + std::log(sumexpnumer) - m2 - std::log(sumexpdenom);
848 
849  // calculate expectations before you resample
850  unsigned int fId(0);
851  //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
852  for(auto & h : fs){
853 
854  Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
855  unsigned int rows = testOutput.rows();
856  unsigned int cols = testOutput.cols();
857  Mat numer = Mat::Zero(rows,cols);
858  float_t denom(0.0);
859  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
860  numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
861  denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
862  }
863  m_expectations[fId] = numer/denom;
864  fId++;
865  }
866 
867  // resample (unnormalized weights ok)
868  if( (m_now+1)%m_rs == 0)
870 
871  // update time step
872  m_now ++;
873  }
874  else //( m_now == 0) // first data point coming
875  {
876  // initialize and update the closed-form mods
877  nsssv tmpMean;
878  nsssMat tmpVar;
879  float_t m1(-std::numeric_limits<float_t>::infinity());
880  for(size_t ii = 0; ii < nparts; ++ii){
881  m_p_samps[ii] = q1Samp(data);
882  tmpMean = initKalmanMean(m_p_samps[ii]);
883  tmpVar = initKalmanVar(m_p_samps[ii]);
884  m_p_innerMods[ii] = kalman<dimnss,dimy,1,float_t>(tmpMean, tmpVar); // TODO: allow for input or check to make sure this doesn't break anything else
885  this->updateKalman(m_p_innerMods[ii], data, m_p_samps[ii]);
886 
887  m_logUnNormWeights[ii] = m_p_innerMods[ii].getLogCondLike() + logMuEv(m_p_samps[ii]) - logQ1Ev(m_p_samps[ii], data);
888 
889  // update a max
890  if(m_logUnNormWeights[ii] > m1)
891  m1 = m_logUnNormWeights[ii];
892  }
893 
894  // calculate log p(y1)
895  float_t sumexp(0.0);
896  for(size_t p = 0; p < nparts; ++p)
897  sumexp += std::exp(m_logUnNormWeights[p] - m1);
898  m_lastLogCondLike = m1 + std::log(sumexp) - std::log(static_cast<float_t>(nparts));
899 
900  // calculate expectations before you resample
901  m_expectations.resize(fs.size());
902  unsigned int fId(0);
903  //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
904  for(auto & h : fs){
905 
906  Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
907  unsigned int rows = testOutput.rows();
908  unsigned int cols = testOutput.cols();
909  Mat numer = Mat::Zero(rows,cols);
910  float_t denom(0.0);
911  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
912  numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
913  denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
914  }
915  m_expectations[fId] = numer/denom;
916  fId++;
917  }
918 
919  // resample (unnormalized weights ok)
920  if( (m_now+1)%m_rs == 0)
922 
923  // advance time step
924  m_now ++;
925  }
926 
927 }
928 
929 
930 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
932 {
933  return m_lastLogCondLike;
934 }
935 
936 
937 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
939 {
940  return m_expectations;
941 }
942 
943 
944 
946 
957 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
958 class rbpf_kalman_bs : public rbpf_base<float_t,dimss,dimnss,dimy>
959 {
960 
961 public:
962 
964  using sssv = Eigen::Matrix<float_t,dimss,1>;
966  using nsssv = Eigen::Matrix<float_t,dimnss,1>;
968  using osv = Eigen::Matrix<float_t,dimy,1>;
970  using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
972  using nsssMat = Eigen::Matrix<float_t,dimnss,dimnss>;
974  using arrayMod = std::array<kalman<dimnss,dimy,0,float_t>,nparts>;
976  using arrayVec = std::array<sssv,nparts>;
978  using arrayfloat_t = std::array<float_t,nparts>;
979 
981 
984  rbpf_kalman_bs(const unsigned int &resamp_sched=1);
985 
986 
990  virtual ~rbpf_kalman_bs();
991 
992 
994 
999  void filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >& fs
1000  = std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >() );
1001 
1003 
1006  float_t getLogCondLike() const;
1007 
1008 
1010 
1014  std::vector<Mat> getExpectations() const;
1015 
1016 
1018 
1022  virtual sssv muSamp() = 0;
1023 
1024 
1026 
1031  virtual nsssv initKalmanMean(const sssv &x21) = 0;
1032 
1033 
1035 
1040  virtual nsssMat initKalmanVar(const sssv &x21) = 0;
1041 
1042 
1044 
1049  virtual sssv fSamp(const sssv &x2tm1) = 0;
1050 
1051 
1053 
1059  virtual void updateKalman(kalman<dimnss, dimy,0,float_t> &kMod, const osv &yt, const sssv &x2t) = 0;
1060 
1061 private:
1062 
1064  unsigned int m_rs;
1072  unsigned int m_now;
1076  resamp_t m_resampler;
1078  std::vector<Mat> m_expectations;
1079 };
1080 
1081 
1082 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
1084  : m_now(0)
1085  , m_lastLogCondLike(0.0)
1086  , m_rs(resamp_sched)
1087 {
1088  std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
1089 }
1090 
1091 
1092 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
1094 
1095 
1096 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
1097 void rbpf_kalman_bs<nparts,dimnss,dimss,dimy,resamp_t,float_t>::filter(const osv &data, const std::vector<std::function<const Mat(const nsssv &x1t, const sssv &x2t)> >& fs)
1098 {
1099 
1100  if(m_now > 0)
1101  {
1102 
1103  // update
1104  sssv newX2Samp;
1105  float_t m1(-std::numeric_limits<float_t>::infinity()); // for updated weights
1106  float_t m2 = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
1107  float_t sumexpdenom(0.0);
1108  for(size_t ii = 0; ii < nparts; ++ii){
1109 
1110  newX2Samp = fSamp(m_p_samps[ii], data);
1111  this->updateKalman(m_p_innerMods[ii], data, newX2Samp);
1112 
1113  // before you update the weights
1114  sumexpdenom += std::exp(m_logUnNormWeights[ii] - m2);
1115 
1116  // update the weights
1117  m_logUnNormWeights[ii] += m_p_innerMods[ii].getLogCondLike();
1118 
1119  // update a max
1120  if(m_logUnNormWeights[ii] > m1)
1121  m1 = m_logUnNormWeights[ii];
1122 
1123  m_p_samps[ii] = newX2Samp;
1124  }
1125 
1126  // calc log p(y_t | y_{1:t-1})
1127  float_t sumexpnumer(0.0);
1128  for(size_t p = 0; p < nparts; ++p)
1129  sumexpnumer += std::exp(m_logUnNormWeights[p] - m1);
1130  m_lastLogCondLike = m1 + std::log(sumexpnumer) - m2 - std::log(sumexpdenom);
1131 
1132  // calculate expectations before you resample
1133  unsigned int fId(0);
1134  //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
1135  for(auto & h : fs){
1136 
1137  Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
1138  unsigned int rows = testOutput.rows();
1139  unsigned int cols = testOutput.cols();
1140  Mat numer = Mat::Zero(rows,cols);
1141  float_t denom(0.0);
1142  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
1143  numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
1144  denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
1145  }
1146  m_expectations[fId] = numer/denom;
1147  fId++;
1148  }
1149 
1150  // resample (unnormalized weights ok)
1151  if( (m_now+1)%m_rs == 0)
1153 
1154  // update time step
1155  m_now ++;
1156  }
1157  else // ( m_now == 0) // first data point coming
1158  {
1159  // initialize and update the closed-form mods
1160  nsssv tmpMean;
1161  nsssMat tmpVar;
1162  float_t m1(-std::numeric_limits<float_t>::infinity());
1163  for(size_t ii = 0; ii < nparts; ++ii){
1164  m_p_samps[ii] = muSamp(data);
1165  tmpMean = initKalmanMean(m_p_samps[ii]);
1166  tmpVar = initKalmanVar(m_p_samps[ii]);
1167  m_p_innerMods[ii] = kalman<dimnss,dimy,1,float_t>(tmpMean, tmpVar); // TODO: allow for input or check to make sure this doesn't break anything else
1168  this->updateKalman(m_p_innerMods[ii], data, m_p_samps[ii]);
1169 
1170  m_logUnNormWeights[ii] = m_p_innerMods[ii].getLogCondLike();
1171 
1172  // update a max
1173  if(m_logUnNormWeights[ii] > m1)
1174  m1 = m_logUnNormWeights[ii];
1175  }
1176 
1177  // calculate log p(y1)
1178  float_t sumexp(0.0);
1179  for(size_t p = 0; p < nparts; ++p)
1180  sumexp += std::exp(m_logUnNormWeights[p] - m1);
1181  m_lastLogCondLike = m1 + std::log(sumexp) - std::log(static_cast<float_t>(nparts));
1182 
1183  // calculate expectations before you resample
1184  m_expectations.resize(fs.size());
1185  unsigned int fId(0);
1186  //float_t m = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
1187  for(auto & h : fs){
1188 
1189  Mat testOutput = h(m_p_innerMods[0].getFilterVec(), m_p_samps[0]);
1190  unsigned int rows = testOutput.rows();
1191  unsigned int cols = testOutput.cols();
1192  Mat numer = Mat::Zero(rows,cols);
1193  float_t denom(0.0);
1194  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
1195  numer += h(m_p_innerMods[prtcl].getFilterVec(), m_p_samps[prtcl])*std::exp(m_logUnNormWeights[prtcl] - m1);
1196  denom += std::exp( m_logUnNormWeights[prtcl] - m1 );
1197  }
1198  m_expectations[fId] = numer/denom;
1199  fId++;
1200  }
1201 
1202  // resample (unnormalized weights ok)
1203  if( (m_now+1)%m_rs == 0)
1205 
1206  // advance time step
1207  m_now ++;
1208  }
1209 
1210 }
1211 
1212 
1213 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
1215 {
1216  return m_lastLogCondLike;
1217 }
1218 
1219 
1220 template<size_t nparts, size_t dimnss, size_t dimss, size_t dimy, typename resamp_t, typename float_t>
1222 {
1223  return m_expectations;
1224 }
1225 
1226 
1227 #endif //RBPF_H
resamp_t m_resampler
Definition: rbpf.h:794
virtual void updateHMM(hmm< dimnss, dimy, float_t > &aModel, const osv &yt, const sssv &x2t)=0
How to update your inner HMM filter object at each time.
std::array< kalman< dimnss, dimy, 0, float_t >, nparts > arrayMod
Definition: rbpf.h:974
virtual nsssv initHMMProbVec(const sssv &x21)=0
Provides the initial mean vector for each HMM filter object.
virtual void updateHMM(hmm< dimnss, dimy, float_t > &aModel, const osv &yt, const sssv &x2t)=0
How to update your inner HMM filter object at each time.
Eigen::Matrix< float_t, Eigen::Dynamic, Eigen::Dynamic > Mat
Definition: rbpf.h:970
arrayfloat_t m_logUnNormWeights
Definition: rbpf.h:788
arrayMod m_p_innerMods
Definition: rbpf.h:1066
Eigen::Matrix< float_t, dimnss, 1 > nsssv
Definition: rbpf.h:34
virtual sssv fSamp(const sssv &x2tm1)=0
Samples the time t second component.
resamp_t m_resampler
Definition: rbpf.h:1076
Eigen::Matrix< float_t, dimnss, 1 > nsssv
Definition: rbpf.h:642
std::array< hmm< dimnss, dimy, float_t >, nparts > arrayMod
Definition: rbpf.h:42
Eigen::Matrix< float_t, Eigen::Dynamic, Eigen::Dynamic > Mat
Definition: rbpf.h:366
arrayMod m_p_innerMods
Definition: rbpf.h:784
unsigned int m_now
Definition: rbpf.h:461
virtual nsssMat initHMMTransMat(const sssv &x21)=0
Provides the transition matrix for each HMM filter object.
float_t getLogCondLike() const
Get the latest log conditional likelihood.
Definition: rbpf.h:1214
Eigen::Matrix< float_t, dimnss, 1 > nsssv
Definition: rbpf.h:966
std::array< float_t, nparts > arrayfloat_t
Definition: rbpf.h:46
std::vector< Mat > m_expectations
Definition: rbpf.h:475
virtual sssv qSamp(const sssv &x2tm1, const osv &yt)=0
Samples the time t second component.
Eigen::Matrix< float_t, Eigen::Dynamic, Eigen::Dynamic > Mat
Definition: rbpf.h:646
Eigen::Matrix< float_t, dimnss, dimnss > nsssMat
Definition: rbpf.h:38
virtual ~rbpf_hmm()
The (virtual) destructor.
Definition: rbpf.h:206
std::array< float_t, nparts > arrayfloat_t
Definition: rbpf.h:654
std::array< sssv, nparts > arrayVec
Definition: rbpf.h:44
virtual nsssMat initHMMTransMat(const sssv &x21)=0
Provides the transition matrix for each HMM filter object.
rbpf_kalman(const unsigned int &resamp_sched=1)
The constructor.
Definition: rbpf.h:801
resamp_t m_resampler
Definition: rbpf.h:473
rbpf_kalman_bs(const unsigned int &resamp_sched=1)
The constructor.
Definition: rbpf.h:1083
std::vector< Mat > m_expectations
Definition: rbpf.h:796
Rao-Blackwellized/Marginal Bootstrap Filter with inner Kalman Filter objectss.
Definition: rbpf.h:958
float_t m_lastLogCondLike
Definition: rbpf.h:792
Eigen::Matrix< float_t, dimy, 1 > osv
Definition: rbpf.h:968
float_t m_lastLogCondLike
Definition: rbpf.h:178
Eigen::Matrix< float_t, dimy, 1 > osv
Definition: rbpf.h:644
std::vector< Mat > m_expectations
Definition: rbpf.h:1078
float_t getLogCondLike() const
Get the latest conditional likelihood.
Definition: rbpf.h:608
resamp_t m_resampler
Definition: rbpf.h:188
float_t m_lastLogCondLike
Definition: rbpf.h:463
A class template for Kalman filtering.
Definition: cf_filters.h:53
virtual void updateKalman(kalman< dimnss, dimy, 0, float_t > &kMod, const osv &yt, const sssv &x2t)=0
How to update your inner Kalman filter object at each time.
float_t m_lastLogCondLike
Definition: rbpf.h:1074
std::array< sssv, nparts > arrayVec
Definition: rbpf.h:370
Eigen::Matrix< float_t, dimss, 1 > sssv
Definition: rbpf.h:358
Eigen::Matrix< float_t, dimnss, dimnss > nsssMat
Definition: rbpf.h:972
float_t getLogCondLike() const
Get the latest log conditional likelihood.
Definition: rbpf.h:931
virtual float_t logFEv(const sssv &x2t, const sssv &x2tm1)=0
Evaluates the state transition density for the second state component.
unsigned int m_now
Definition: rbpf.h:790
arrayfloat_t m_logUnNormWeights
Definition: rbpf.h:471
virtual ~rbpf_hmm_bs()
The (virtual) destructor.
Definition: rbpf.h:491
Eigen::Matrix< float_t, dimy, 1 > osv
Definition: rbpf.h:362
Eigen::Matrix< float_t, dimnss, 1 > nsssv
Definition: rbpf.h:360
virtual nsssMat initKalmanVar(const sssv &x21)=0
Provides the initial covariance matrix for each Kalman filter object.
void filter(const osv &data, const std::vector< std::function< const Mat(const nsssv &x1tProbs, const sssv &x2t)> > &fs=std::vector< std::function< const Mat(const nsssv &, const sssv &)> >())
Filter.
Definition: rbpf.h:495
arrayVec m_p_samps
Definition: rbpf.h:1068
virtual ~rbpf_kalman_bs()
The (virtual) destructor.
Definition: rbpf.h:1093
unsigned int m_now
Definition: rbpf.h:1072
std::vector< Mat > m_expectations
Definition: rbpf.h:190
Rao-Blackwellized/Marginal Particle Filter with inner Kalman Filter objectss.
Definition: rbpf.h:634
std::vector< Mat > getExpectations() const
Get the latest filtered expectation E[h(x_1t, x_2t) | y_{1:t}].
Definition: rbpf.h:1221
virtual float_t logFEv(const sssv &x2t, const sssv &x2tm1)=0
Evaluates the state transition density for the second state component.
unsigned int m_rs
Definition: rbpf.h:1064
arrayVec m_p_samps
Definition: rbpf.h:469
float_t getLogCondLike() const
Get the latest conditional likelihood.
Definition: rbpf.h:327
void filter(const osv &data, const std::vector< std::function< const Mat(const nsssv &x1tProbs, const sssv &x2t)> > &fs=std::vector< std::function< const Mat(const nsssv &, const sssv &)> >())
Filter.
Definition: rbpf.h:210
virtual sssv muSamp()=0
Sample from the first sampler.
std::array< sssv, nparts > arrayVec
Definition: rbpf.h:652
std::array< float_t, nparts > arrayfloat_t
Definition: rbpf.h:372
virtual nsssv initHMMProbVec(const sssv &x21)=0
Provides the initial mean vector for each HMM filter object.
virtual sssv q1Samp(const osv &y1)=0
Sample from the first sampler.
A class template for HMM filtering.
Definition: cf_filters.h:362
Eigen::Matrix< float_t, dimss, 1 > sssv
Definition: rbpf.h:32
arrayMod m_p_innerMods
Definition: rbpf.h:182
All non Rao-Blackwellized particle filters inherit from this.
std::array< kalman< dimnss, dimy, 0, float_t >, nparts > arrayMod
Definition: rbpf.h:650
virtual float_t logMuEv(const sssv &x21)=0
Evaluates the first time state density.
std::vector< Mat > getExpectations() const
Get vector of expectations.
Definition: rbpf.h:334
virtual nsssMat initKalmanVar(const sssv &x21)=0
Provides the initial covariance matrix for each Kalman filter object.
unsigned int m_rs
Definition: rbpf.h:180
void filter(const osv &data, const std::vector< std::function< const Mat(const nsssv &x1t, const sssv &x2t)> > &fs=std::vector< std::function< const Mat(const nsssv &x1t, const sssv &x2t)> >())
Filter!
Definition: rbpf.h:815
rbpf_hmm(const unsigned int &resamp_sched=1)
The constructor.
Definition: rbpf.h:196
arrayfloat_t m_logUnNormWeights
Definition: rbpf.h:186
virtual float_t logQ1Ev(const sssv &x21, const osv &y1)=0
Evaluates the proposal density of the second state component at time 1.
Eigen::Matrix< float_t, dimnss, dimnss > nsssMat
Definition: rbpf.h:648
virtual sssv qSamp(const sssv &x2tm1, const osv &yt)=0
Samples the time t second component.
virtual sssv q1Samp(const osv &y1)=0
Sample from the first time&#39;s proposal distribution.
arrayVec m_p_samps
Definition: rbpf.h:786
Eigen::Matrix< float_t, dimss, 1 > sssv
Definition: rbpf.h:964
rbpf_hmm_bs(const unsigned int &resamp_sched=1)
The constructor.
Definition: rbpf.h:481
unsigned int m_rs
Definition: rbpf.h:465
virtual float_t logQEv(const sssv &x2t, const sssv &x2tm1, const osv &yt)=0
Evaluates the proposal density at time t > 1.
Definition: pf_base.h:71
Eigen::Matrix< float_t, Eigen::Dynamic, Eigen::Dynamic > Mat
Definition: rbpf.h:40
virtual float_t logMuEv(const sssv &x21)=0
Evaluates the first time state density.
virtual void updateKalman(kalman< dimnss, dimy, 0, float_t > &kMod, const osv &yt, const sssv &x2t)=0
How to update your inner Kalman filter object at each time.
Rao-Blackwellized/Marginal Particle Filter with inner HMMs.
Definition: rbpf.h:27
std::array< float_t, nparts > arrayfloat_t
Definition: rbpf.h:978
std::vector< Mat > getExpectations() const
Get vector of expectations.
Definition: rbpf.h:615
std::array< hmm< dimnss, dimy, float_t >, nparts > arrayMod
Definition: rbpf.h:368
virtual nsssv initKalmanMean(const sssv &x21)=0
Provides the initial mean vector for each Kalman filter object.
Eigen::Matrix< float_t, dimss, 1 > sssv
Definition: rbpf.h:640
virtual nsssv initKalmanMean(const sssv &x21)=0
Provides the initial mean vector for each Kalman filter object.
unsigned int m_now
Definition: rbpf.h:176
forces structure on the closed-form filters.
std::array< sssv, nparts > arrayVec
Definition: rbpf.h:976
void filter(const osv &data, const std::vector< std::function< const Mat(const nsssv &x1t, const sssv &x2t)> > &fs=std::vector< std::function< const Mat(const nsssv &x1t, const sssv &x2t)> >())
Filter!
Definition: rbpf.h:1097
virtual float_t logQEv(const sssv &x2t, const sssv &x2tm1, const osv &yt)=0
Evaluates the proposal density at time t > 1.
Eigen::Matrix< float_t, dimnss, dimnss > nsssMat
Definition: rbpf.h:364
Rao-Blackwellized/Marginal Bootstrap Filter with inner HMMs.
Definition: rbpf.h:353
virtual float_t logQ1Ev(const sssv &x21, const osv &y1)=0
Evaluates the proposal density of the second state component at time 1.
virtual sssv muSamp()=0
Sample from the first time&#39;s proposal distribution.
Eigen::Matrix< float_t, dimy, 1 > osv
Definition: rbpf.h:36
arrayVec m_p_samps
Definition: rbpf.h:184
std::vector< Mat > getExpectations() const
Get the latest filtered expectation E[h(x_1t, x_2t) | y_{1:t}].
Definition: rbpf.h:938
arrayMod m_p_innerMods
Definition: rbpf.h:467
unsigned int m_rs
Definition: rbpf.h:782
arrayfloat_t m_logUnNormWeights
Definition: rbpf.h:1070
virtual sssv fSamp(const sssv &x2tm1)=0
Samples the time t second component.