pf
bootstrap_filter_with_covariates.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 
23 template<size_t nparts, size_t dimx, size_t dimy, size_t dimcov, typename resamp_t, typename float_t>
24 class BSFilterWC
25 {
26 public:
27 
29  using ssv = Eigen::Matrix<float_t, dimx, 1>;
31  using osv = Eigen::Matrix<float_t, dimy, 1>; // obs size vec
33  using cvsv = Eigen::Matrix<float_t,dimcov,1>;
35  using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
37  using arrayStates = std::array<ssv, nparts>;
39  using arrayfloat_t = std::array<float_t, nparts>;
41  using Funcs = std::vector<std::function<const Mat(const ssv&, const cvsv&)>>;
42 
47  BSFilterWC(const unsigned int &rs = 1);
48 
49 
53  virtual ~BSFilterWC();
54 
55 
60  float_t getLogCondLike() const;
61 
62 
70  void filter(const osv &ydata, const cvsv &covdata, const Funcs& fs = Funcs());
71 
72 
77  auto getExpectations () const -> std::vector<Mat>;
78 
79 
86  virtual float_t logMuEv (const ssv &x1, const cvsv &z1) = 0;
87 
88 
95  virtual ssv q1Samp (const osv &y1, const cvsv &z1) = 0;
96 
97 
105  virtual float_t logQ1Ev (const ssv &x1, const osv &y1, const cvsv &z1) = 0;
106 
107 
115  virtual float_t logGEv (const osv &yt, const ssv &xt, const cvsv &zt) = 0;
116 
117 
119 
125  virtual ssv fSamp (const ssv &xtm1, const cvsv &zt) = 0;
126 
127 protected:
130 
133 
135  unsigned int m_now;
136 
139 
141  resamp_t m_resampler;
142 
144  std::vector<Mat> m_expectations;
145 
147  unsigned int m_resampSched;
148 };
149 
150 
151 
155 
156 template<size_t nparts, size_t dimx, size_t dimy, size_t dimcov, typename resamp_t, typename float_t>
157 BSFilterWC<nparts, dimx, dimy, dimcov, resamp_t, float_t>::BSFilterWC(const unsigned int &rs)
158  : m_now(0)
159  , m_logLastCondLike(0.0)
160  , m_resampSched(rs)
161 
162 {
163  std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0); // log(1) = 0
164 }
165 
166 
167 template<size_t nparts, size_t dimx, size_t dimy, size_t dimcov, typename resamp_t, typename float_t>
169 
170 
171 template<size_t nparts, size_t dimx, size_t dimy, size_t dimcov, typename resamp_t, typename float_t>
173 {
174 
175  if (m_now == 0) //time 1
176  {
177  // only need to iterate over particles once
178  for(size_t ii = 0; ii < nparts; ++ii)
179  {
180  // sample particles
181  m_particles[ii] = q1Samp(dat, covData);
182  m_logUnNormWeights[ii] = logMuEv(m_particles[ii], covData);
183  m_logUnNormWeights[ii] += logGEv(dat, m_particles[ii], covData);
184  m_logUnNormWeights[ii] -= logQ1Ev(m_particles[ii], dat, covData);
185  }
186 
187  // calculate log cond likelihood with log-exp-sum trick
188  float_t max = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
189  float_t sumExp(0.0);
190  for(size_t i = 0; i < nparts; ++i){
191  sumExp += std::exp(m_logUnNormWeights[i] - max);
192  }
193  m_logLastCondLike = -std::log(nparts) + max + std::log(sumExp);
194 
195  // calculate expectations before you resample
196  // paying mind to underflow
197  m_expectations.resize(fs.size());
198  unsigned int fId(0);
199  for(auto & h : fs){
200 
201  Mat testOutput = h(m_particles[0], covData);
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){ // iterate over all particles
207  numer += h(m_particles[prtcl],covData) * std::exp( m_logUnNormWeights[prtcl] - (max) );
208  weightNormConst += std::exp( m_logUnNormWeights[prtcl] - (max) );
209  }
210  m_expectations[fId] = numer/weightNormConst;
211  fId++;
212  }
213 
214  // resample if you should
215  if ( (m_now+1) % m_resampSched == 0){
217  }
218 
219  // advance time step
220  m_now += 1;
221  }
222  else // m_now > 0
223  {
224 
225  // try to iterate over particles all at once
226  ssv newSamp;
227  float_t maxOldLogUnNormWts(-std::numeric_limits<float_t>::infinity());
228  arrayfloat_t oldLogUnNormWts = m_logUnNormWeights;
229  for(size_t ii = 0; ii < nparts; ++ii)
230  {
231  // update max of old logUnNormWts
232  if (m_logUnNormWeights[ii] > maxOldLogUnNormWts)
233  maxOldLogUnNormWts = m_logUnNormWeights[ii];
234 
235  // sample and get weight adjustments
236  newSamp = fSamp(m_particles[ii], covData);
237  m_logUnNormWeights[ii] = logGEv(dat, newSamp, covData);
238 
239  // overwrite stuff
240  m_particles[ii] = newSamp;
241  }
242 
243  // compute estimate of log p(y_t|y_{1:t-1}) with log-exp-sum trick
244  float_t maxNumer = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end()); //because you added log adjustments
245  float_t sumExp1(0.0);
246  float_t sumExp2(0.0);
247  for(size_t i = 0; i < nparts; ++i){
248  sumExp1 += std::exp(m_logUnNormWeights[i] - maxNumer);
249  sumExp2 += std::exp(oldLogUnNormWts[i] - maxOldLogUnNormWts); //1
250  }
251  m_logLastCondLike = maxNumer + std::log(sumExp1) - maxOldLogUnNormWts - std::log(sumExp2);
252 
253  // calculate expectations before you resample
254  int fId(0);
255  for(auto & h : fs){ // iterate over all functions
256 
257  Mat testOutput = h(m_particles[0],covData);
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){ // iterate over all particles
263  numer += h(m_particles[prtcl],covData) * std::exp(m_logUnNormWeights[prtcl] - maxNumer);
264  weightNormConst += std::exp(m_logUnNormWeights[prtcl] - maxNumer);
265  }
266  m_expectations[fId] = numer/weightNormConst;
267  fId++;
268  }
269 
270  // resample if you should
271  if ( (m_now+1) % m_resampSched == 0)
273 
274  // advance time
275  m_now += 1;
276  }
277 }
278 
279 
280 template<size_t nparts, size_t dimx, size_t dimy, size_t dimcov, typename resamp_t, typename float_t>
282 {
283  return m_logLastCondLike;
284 }
285 
286 
287 template<size_t nparts, size_t dimx, size_t dimy, size_t dimcov, typename resamp_t, typename float_t>
289 {
290  return m_expectations;
291 }
292 
293 
294 
295 #endif // BOOTSTRAP_FILTER_H
virtual float_t logGEv(const osv &yt, const ssv &xt, const cvsv &zt)=0
Calculate gEv or logGEv.
std::vector< std::function< const Mat(const ssv &, const cvsv &)> > Funcs
Definition: bootstrap_filter_with_covariates.h:41
Eigen::Matrix< float_t, dimy, 1 > osv
Definition: bootstrap_filter_with_covariates.h:31
virtual ssv q1Samp(const osv &y1, const cvsv &z1)=0
Samples from time 1 proposal.
float_t m_logLastCondLike
log p(y_t|y_{1:t-1}) or log p(y1)
Definition: bootstrap_filter_with_covariates.h:138
A base class for the bootstrap particle filter with covariates.
Definition: bootstrap_filter_with_covariates.h:24
unsigned int m_resampSched
resampling schedule (e.g. resample every __ time points)
Definition: bootstrap_filter_with_covariates.h:147
virtual ssv fSamp(const ssv &xtm1, const cvsv &zt)=0
Sample from the state transition distribution.
Eigen::Matrix< float_t, dimx, 1 > ssv
Definition: bootstrap_filter_with_covariates.h:29
std::vector< Mat > m_expectations
expectations E[h(x_t) | y_{1:t}] for user defined "h"s
Definition: bootstrap_filter_with_covariates.h:144
std::array< float_t, nparts > arrayfloat_t
Definition: bootstrap_filter_with_covariates.h:39
Eigen::Matrix< float_t, dimcov, 1 > cvsv
Definition: bootstrap_filter_with_covariates.h:33
arrayfloat_t m_logUnNormWeights
particle unnormalized weights
Definition: bootstrap_filter_with_covariates.h:132
virtual float_t logQ1Ev(const ssv &x1, const osv &y1, const cvsv &z1)=0
Calculate q1Ev or log q1Ev.
virtual ~BSFilterWC()
The (virtual) destructor.
Definition: bootstrap_filter_with_covariates.h:168
float_t getLogCondLike() const
Returns the most recent (log-) conditiona likelihood.
Definition: bootstrap_filter_with_covariates.h:281
std::array< ssv, nparts > arrayStates
Definition: bootstrap_filter_with_covariates.h:37
resamp_t m_resampler
resampler object
Definition: bootstrap_filter_with_covariates.h:141
unsigned int m_now
time point
Definition: bootstrap_filter_with_covariates.h:135
All non Rao-Blackwellized particle filters inherit from this.
Eigen::Matrix< float_t, Eigen::Dynamic, Eigen::Dynamic > Mat
Definition: bootstrap_filter_with_covariates.h:35
void filter(const osv &ydata, const cvsv &covdata, const Funcs &fs=Funcs())
updates filtering distribution on a new datapoint. Optionally stores expectations of functionals...
Definition: bootstrap_filter_with_covariates.h:172
arrayStates m_particles
particle samples
Definition: bootstrap_filter_with_covariates.h:129
auto getExpectations() const -> std::vector< Mat >
return all stored expectations (taken with respect to $p(x_t|y_{1:t})$
Definition: bootstrap_filter_with_covariates.h:288
BSFilterWC(const unsigned int &rs=1)
The constructor.
Definition: bootstrap_filter_with_covariates.h:157
virtual float_t logMuEv(const ssv &x1, const cvsv &z1)=0
Calculate muEv or logmuEv.