pf
sisr_filter.h
Go to the documentation of this file.
1 #ifndef SISR_FILTER_H
2 #define SISR_FILTER_H
3 
4 #include <array>
5 #include <Eigen/Dense>
6 
7 #include "pf_base.h"
8 
10 
20 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug=false>
21 class SISRFilter : public pf_base<float_t, dimy, dimx>
22 {
23 public:
24 
26  using ssv = Eigen::Matrix<float_t, dimx, 1>;
28  using osv = Eigen::Matrix<float_t, dimy, 1>; // obs size vec
30  using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
32  using arrayStates = std::array<ssv, nparts>;
34  using arrayfloat_t = std::array<float_t, nparts>;
36  static constexpr unsigned int num_particles = nparts;
37 
38 
43  SISRFilter(const unsigned int &rs=1);
44 
45 
49  virtual ~SISRFilter();
50 
51 
56  float_t getLogCondLike() const;
57 
58 
63  std::vector<Mat> getExpectations() const;
64 
65 
72  void filter(const osv &data, const std::vector<std::function<const Mat(const ssv&)> >& fs = std::vector<std::function<const Mat(const ssv&)> >());
73 
74 
80  virtual float_t logMuEv (const ssv &x1) = 0;
81 
82 
88  virtual ssv q1Samp (const osv &y1) = 0;
89 
90 
97  virtual float_t logQ1Ev (const ssv &x1, const osv &y1 ) = 0;
98 
99 
106  virtual float_t logGEv (const osv &yt, const ssv &xt ) = 0;
107 
108 
115  virtual float_t logFEv (const ssv &xt, const ssv &xtm1 ) = 0;
116 
117 
124  virtual ssv qSamp (const ssv &xtm1, const osv &yt ) = 0;
125 
126 
134  virtual float_t logQEv (const ssv &xt, const ssv &xtm1, const osv &yt ) = 0;
135 
136 private:
137 
140 
143 
145  unsigned int m_now;
146 
149 
151  resamp_t m_resampler;
152 
154  std::vector<Mat> m_expectations; // stores any sample averages the user wants
155 
157  unsigned int m_resampSched;
158 
159 
164 };
165 
166 
167 
168 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
170  : m_now(0)
171  , m_logLastCondLike(0.0)
172  , m_resampSched(rs)
173 {
174  std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0); // log(1) = 0
175 }
176 
177 
178 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
180 
181 
182 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
184 {
185  return m_logLastCondLike;
186 }
187 
188 
189 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
191 {
192  return m_expectations;
193 }
194 
195 
196 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
197 void SISRFilter<nparts,dimx,dimy,resamp_t,float_t, debug>::filter(const osv &data, const std::vector<std::function<const Mat(const ssv&)> >& fs)
198 {
199 
200  if(m_now > 0)
201  {
202 
203  // try to iterate over particles all at once
204  ssv newSamp;
205  arrayfloat_t oldLogUnNormWts = m_logUnNormWeights;
206  float_t maxOldLogUnNormWts(-std::numeric_limits<float_t>::infinity());
207  for(size_t ii = 0; ii < nparts; ++ii)
208  {
209 
210  // update max of old logUnNormWts before you change the element
211  if (m_logUnNormWeights[ii] > maxOldLogUnNormWts)
212  maxOldLogUnNormWts = m_logUnNormWeights[ii];
213 
214  // sample and get weight adjustments
215  newSamp = qSamp(m_particles[ii], data);
216  m_logUnNormWeights[ii] = logFEv(newSamp, m_particles[ii]);
217  m_logUnNormWeights[ii] += logGEv(data, newSamp);
218  m_logUnNormWeights[ii] -= logQEv(newSamp, m_particles[ii], data);
219 
220  // overwrite stuff
221  m_particles[ii] = newSamp;
222 
223  if constexpr(debug)
224  std::cout << "time: " << m_now << ", transposed sample: " << m_particles[ii].transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
225 
226  }
227 
228  // compute estimate of log p(y_t|y_{1:t-1}) with log-exp-sum trick
229  float_t maxNumer = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end()); //because you added log adjustments
230  float_t sumExp1(0.0);
231  float_t sumExp2(0.0);
232  for(size_t i = 0; i < nparts; ++i){
233  sumExp1 += std::exp(m_logUnNormWeights[i] - maxNumer);
234  sumExp2 += std::exp(oldLogUnNormWts[i] - maxOldLogUnNormWts);
235  }
236  m_logLastCondLike = maxNumer + std::log(sumExp1) - maxOldLogUnNormWts - std::log(sumExp2);
237 
238  // calculate expectations before you resample
239  unsigned int fId(0);
240  float_t weightNormConst(0.0);
241  for(auto & h : fs){ // iterate over all functions
242 
243  Mat testOut = h(m_particles[0]);
244  unsigned int rows = testOut.rows();
245  unsigned int cols = testOut.cols();
246  Mat numer = Mat::Zero(rows,cols);
247  float_t denom(0.0);
248 
249  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){ // iterate over all particles
250  numer += h(m_particles[prtcl]) * std::exp(m_logUnNormWeights[prtcl]);
251  denom += std::exp(m_logUnNormWeights[prtcl]);
252  }
253  m_expectations[fId] = numer/denom;
254 
255  // print stuff if debug mode is on
256  if constexpr(debug)
257  std::cout << "transposed expectation " << fId << ": " << m_expectations[fId].transpose() << "\n";
258 
259  fId++;
260  }
261 
262  // resample if you should
263  if( (m_now + 1) % m_resampSched == 0)
265 
266  // advance time
267  m_now += 1;
268  }
269  else // (m_now == 0) //time 1
270  {
271 
272  // only need to iterate over particles once
273  float_t sumWts(0.0);
274  for(size_t ii = 0; ii < nparts; ++ii)
275  {
276  // sample particles
277  m_particles[ii] = q1Samp(data);
279  m_logUnNormWeights[ii] += logGEv(data, m_particles[ii]);
280  m_logUnNormWeights[ii] -= logQ1Ev(m_particles[ii], data);
281 
282  if constexpr(debug)
283  std::cout << "time: " << m_now << ", transposed sample: " << m_particles[ii].transpose() << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
284 
285 
286  }
287 
288  // calculate log cond likelihood with log-exp-sum trick
289  float_t max = *std::max_element(m_logUnNormWeights.begin(), m_logUnNormWeights.end());
290  float_t sumExp(0.0);
291  for(size_t i = 0; i < nparts; ++i){
292  sumExp += std::exp(m_logUnNormWeights[i] - max);
293  }
294  m_logLastCondLike = -std::log(nparts) + max + std::log(sumExp);
295 
296  // calculate expectations before you resample
297  m_expectations.resize(fs.size());
298  unsigned int fId(0);
299  for(auto & h : fs){
300 
301  Mat testOut = h(m_particles[0]);
302  unsigned int rows = testOut.rows();
303  unsigned int cols = testOut.cols();
304  Mat numer = Mat::Zero(rows,cols);
305  float_t denom(0.0);
306 
307  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
308  numer += h(m_particles[prtcl]) * std::exp(m_logUnNormWeights[prtcl]);
309  denom += std::exp(m_logUnNormWeights[prtcl]);
310  }
311  m_expectations[fId] = numer/denom;
312 
313  // print stuff if debug mode is on
314  if constexpr(debug)
315  std::cout << "transposed expectation " << fId << ": " << m_expectations[fId].transpose() << "\n";
316 
317  fId++;
318  }
319 
320  // resample if you should
321  if( (m_now + 1) % m_resampSched == 0)
323 
324  // advance time step
325  m_now += 1;
326  }
327 
328 }
329 
330 
331 
332 
333 
334 
335 
336 
337 #endif //SISR_FILTER_H
std::array< ssv, nparts > arrayStates
Definition: sisr_filter.h:32
arrayfloat_t m_logUnNormWeights
particle weights
Definition: sisr_filter.h:142
unsigned int m_now
current time point
Definition: sisr_filter.h:145
std::vector< Mat > m_expectations
expectations E[h(x_t) | y_{1:t}] for user defined "h"s
Definition: sisr_filter.h:154
float_t m_logLastCondLike
log p(y_t|y_{1:t-1}) or log p(y1)
Definition: sisr_filter.h:148
Eigen::Matrix< float_t, dimy, 1 > osv
Definition: sisr_filter.h:28
Definition: pf_base.h:19
resamp_t m_resampler
resampling object
Definition: sisr_filter.h:151
SISRFilter(const unsigned int &rs=1)
The (one and only) constructor.
Definition: sisr_filter.h:169
virtual ~SISRFilter()
The (virtual) destructor.
Definition: sisr_filter.h:179
virtual float_t logQ1Ev(const ssv &x1, const osv &y1)=0
Calculate q1Ev or log q1Ev.
Eigen::Matrix< float_t, dimx, 1 > ssv
Definition: sisr_filter.h:26
std::array< float_t, nparts > arrayfloat_t
Definition: sisr_filter.h:34
Eigen::Matrix< float_t, Eigen::Dynamic, Eigen::Dynamic > Mat
Definition: sisr_filter.h:30
virtual ssv q1Samp(const osv &y1)=0
Samples from time 1 proposal.
virtual ssv qSamp(const ssv &xtm1, const osv &yt)=0
Samples from the proposal/instrumental/importance density at time t.
All non Rao-Blackwellized particle filters inherit from this.
static constexpr unsigned int num_particles
Definition: sisr_filter.h:36
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: sisr_filter.h:197
virtual float_t logQEv(const ssv &xt, const ssv &xtm1, const osv &yt)=0
Evaluates the proposal/instrumental/importance density/pmf.
virtual float_t logFEv(const ssv &xt, const ssv &xtm1)=0
Evaluates the state transition density.
float_t getLogCondLike() const
Returns the most recent (log-) conditiona likelihood.
Definition: sisr_filter.h:183
std::vector< Mat > getExpectations() const
return all stored expectations (taken with respect to $p(x_t|y_{1:t})$
Definition: sisr_filter.h:190
virtual float_t logGEv(const osv &yt, const ssv &xt)=0
Calculate gEv or logGEv.
virtual float_t logMuEv(const ssv &x1)=0
Calculate muEv or logmuEv.
A base class for the Sequential Important Sampling with Resampling (SISR).
Definition: sisr_filter.h:21
unsigned int m_resampSched
resampling schedule (e.g. resample every __ time points)
Definition: sisr_filter.h:157
arrayStates m_particles
particle samples
Definition: sisr_filter.h:139