pf
bootstrap_filter.h
Go to the documentation of this file.
1 #ifndef BOOTSTRAP_FILTER_H
2 #define BOOTSTRAP_FILTER_H
3 
4 #include <array>
5 #include <vector>
6 #include <Eigen/Dense>
7 
8 #include "pf_base.h"
9 
10 
12 
22 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug=false>
23 class BSFilter : public pf_base<float_t, dimy, dimx>
24 {
25 public:
26 
28  using ssv = Eigen::Matrix<float_t, dimx, 1>;
30  using osv = Eigen::Matrix<float_t, dimy, 1>; // obs size vec
32  using Mat = Eigen::Matrix<float_t, Eigen::Dynamic, Eigen::Dynamic>;
34  using arrayStates = std::array<ssv, nparts>;
36  using arrayFloat = std::array<float_t, nparts>;
38  static constexpr unsigned int num_particles = nparts;
39 
40 
45  BSFilter(const unsigned int &rs = 1);
46 
47 
51  virtual ~BSFilter();
52 
53 
58  float_t getLogCondLike() const;
59 
60 
67  void filter(const osv &data, const std::vector<std::function<const Mat(const ssv&)> >& fs = std::vector<std::function<const Mat(const ssv&)> >());
68 
69 
74  auto getExpectations () const -> std::vector<Mat>;
75 
76 
82  virtual float_t logMuEv (const ssv &x1) = 0;
83 
84 
90  virtual ssv q1Samp (const osv &y1) = 0;
91 
92 
99  virtual float_t logQ1Ev (const ssv &x1, const osv &y1 ) = 0;
100 
101 
108  virtual float_t logGEv (const osv &yt, const ssv &xt ) = 0;
109 
110 
112 
117  virtual ssv fSamp (const ssv &xtm1) = 0;
118 
119 protected:
122 
125 
127  unsigned int m_now;
128 
131 
133  resamp_t m_resampler;
134 
136  std::vector<Mat> m_expectations;
137 
139  unsigned int m_resampSched;
140 };
141 
142 
143 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
144 BSFilter<nparts, dimx, dimy, resamp_t, float_t, debug>::BSFilter(const unsigned int &rs)
145  : m_now(0)
146  , m_logLastCondLike(0.0)
147  , m_resampSched(rs)
148 
149 {
150  std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
151 }
152 
153 
154 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
156 
157 
158 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
159 void BSFilter<nparts, dimx, dimy, resamp_t, float_t, debug>::filter(const osv &dat, const std::vector<std::function<const Mat(const ssv&)> >& fs)
160 {
161 
162  if( m_now > 0)
163  {
164 
165  // try to iterate over particles all at once
166  ssv newSamp;
167  float_t maxOldLogUnNormWts(-std::numeric_limits<float_t>::infinity());
168  arrayFloat oldLogUnNormWts = m_logUnNormWeights;
169  for(size_t ii = 0; ii < nparts; ++ii)
170  {
171  // update max of old logUnNormWts
172  if (m_logUnNormWeights[ii] > maxOldLogUnNormWts)
173  maxOldLogUnNormWts = m_logUnNormWeights[ii];
174 
175  // sample and get weight adjustments
176  newSamp = fSamp(m_particles[ii]);
177  m_logUnNormWeights[ii] = logGEv(dat, newSamp);
178 
179  // overwrite stuff
180  m_particles[ii] = newSamp;
181 
182  // print stuff if debug mode is on
183  if constexpr(debug)
184  std::cout << "time: " << m_now << ", transposed sample: " << m_particles[ii].transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
185  }
186 
187  // compute estimate of log p(y_t|y_{1:t-1}) with log-exp-sum trick
188  float_t maxNumer = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end()); //because you added log adjustments
189  float_t sumExp1(0.0);
190  float_t sumExp2(0.0);
191  for(size_t i = 0; i < nparts; ++i){
192  sumExp1 += std::exp(m_logUnNormWeights[i] - maxNumer);
193  sumExp2 += std::exp(oldLogUnNormWts[i] - maxOldLogUnNormWts); //1
194  }
195  m_logLastCondLike = maxNumer + std::log(sumExp1) - maxOldLogUnNormWts - std::log(sumExp2);
196 
197  // calculate expectations before you resample
198  int fId(0);
199  for(auto & h : fs){
200 
201  Mat testOutput = h(m_particles[0]);
202  unsigned int rows = testOutput.rows();
203  unsigned int cols = testOutput.cols();
204  Mat numer = Mat::Zero(rows,cols);
205  float_t weightNormConst (0.0);
206  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
207  numer += h(m_particles[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - maxNumer);
208  weightNormConst += std::exp(m_logUnNormWeights[prtcl] - maxNumer);
209  }
210  m_expectations[fId] = numer/weightNormConst;
211 
212  // print stuff if debug mode is on
213  if constexpr(debug)
214  std::cout << "transposed expectation " << fId << ": " << m_expectations[fId].transpose() << "\n";
215 
216  fId++;
217  }
218 
219  // resample if you should
220  if ( (m_now+1) % m_resampSched == 0)
222 
223  // advance time
224  m_now += 1;
225  }
226  else // (m_now == 0) //time 1
227  {
228  // only need to iterate over particles once
229  for(size_t ii = 0; ii < nparts; ++ii)
230  {
231  // sample particles
232  m_particles[ii] = q1Samp(dat);
234  m_logUnNormWeights[ii] += logGEv(dat, m_particles[ii]);
235  m_logUnNormWeights[ii] -= logQ1Ev(m_particles[ii], dat);
236 
237  // print stuff if debug mode is on
238  if constexpr(debug)
239  std::cout << "time: " << m_now << ", transposed sample: " << m_particles[ii].transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
240 
241  }
242 
243  // calculate log cond likelihood with log-exp-sum trick
244  float_t max = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
245  float_t sumExp(0.0);
246  for(size_t i = 0; i < nparts; ++i){
247  sumExp += std::exp(m_logUnNormWeights[i] - max);
248  }
249  m_logLastCondLike = -std::log(nparts) + (max) + std::log(sumExp);
250 
251  // calculate expectations before you resample
252  // paying mind to underflow
253  m_expectations.resize(fs.size());
254  unsigned int fId(0);
255  for(auto & h : fs){
256 
257  Mat testOutput = h(m_particles[0]);
258  unsigned int rows = testOutput.rows();
259  unsigned int cols = testOutput.cols();
260  Mat numer = Mat::Zero(rows,cols);
261  float_t weightNormConst (0.0);
262  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
263  numer += h(m_particles[prtcl]) * std::exp( m_logUnNormWeights[prtcl] - (max) );
264  weightNormConst += std::exp( m_logUnNormWeights[prtcl] - (max) );
265  }
266  m_expectations[fId] = numer/weightNormConst;
267 
268  // print stuff if debug mode is on
269  if constexpr(debug)
270  std::cout << "transposed expectation " << fId << ": " << m_expectations[fId].transpose() << "\n";
271 
272  fId++;
273  }
274 
275  // resample if you should
276  if ( (m_now+1) % m_resampSched == 0){
278  }
279 
280  // advance time step
281  m_now += 1;
282  }
283 
284 }
285 
286 
287 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
289 {
290  return m_logLastCondLike;
291 }
292 
293 
294 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
296 {
297  return m_expectations;
298 }
299 
300 
301 
302 #endif // BOOTSTRAP_FILTER_H
virtual float_t logMuEv(const ssv &x1)=0
Calculate muEv or logmuEv.
Eigen::Matrix< float_t, dimx, 1 > ssv
Definition: bootstrap_filter.h:28
Eigen::Matrix< float_t, dimy, 1 > osv
Definition: bootstrap_filter.h:30
virtual float_t logQ1Ev(const ssv &x1, const osv &y1)=0
Calculate q1Ev or log q1Ev.
Definition: pf_base.h:19
std::array< ssv, nparts > arrayStates
Definition: bootstrap_filter.h:34
resamp_t m_resampler
resampler object
Definition: bootstrap_filter.h:133
virtual ssv fSamp(const ssv &xtm1)=0
Sample from the state transition distribution.
std::vector< Mat > m_expectations
expectations E[h(x_t) | y_{1:t}] for user defined "h"s
Definition: bootstrap_filter.h:136
void filter(const osv &data, const std::vector< std::function< const Mat(const ssv &)> > &fs=std::vector< std::function< const Mat(const ssv &)> >())
updates filtering distribution on a new datapoint. Optionally stores expectations of functionals...
Definition: bootstrap_filter.h:159
BSFilter(const unsigned int &rs=1)
The constructor.
Definition: bootstrap_filter.h:144
auto getExpectations() const -> std::vector< Mat >
return all stored expectations (taken with respect to $p(x_t|y_{1:t})$
Definition: bootstrap_filter.h:295
virtual ~BSFilter()
The (virtual) destructor.
Definition: bootstrap_filter.h:155
Eigen::Matrix< float_t, Eigen::Dynamic, Eigen::Dynamic > Mat
Definition: bootstrap_filter.h:32
virtual ssv q1Samp(const osv &y1)=0
Samples from time 1 proposal.
static constexpr unsigned int num_particles
Definition: bootstrap_filter.h:38
All non Rao-Blackwellized particle filters inherit from this.
arrayFloat m_logUnNormWeights
particle unnormalized weights
Definition: bootstrap_filter.h:124
float_t getLogCondLike() const
Returns the most recent (log-) conditiona likelihood.
Definition: bootstrap_filter.h:288
A base class for the bootstrap particle filter.
Definition: bootstrap_filter.h:23
float_t m_logLastCondLike
log p(y_t|y_{1:t-1}) or log p(y1)
Definition: bootstrap_filter.h:130
unsigned int m_resampSched
resampling schedule (e.g. resample every __ time points)
Definition: bootstrap_filter.h:139
std::array< float_t, nparts > arrayFloat
Definition: bootstrap_filter.h:36
virtual float_t logGEv(const osv &yt, const ssv &xt)=0
Calculate gEv or logGEv.
arrayStates m_particles
particle samples
Definition: bootstrap_filter.h:121
unsigned int m_now
time point
Definition: bootstrap_filter.h:127