pf
auxiliary_pf.h
Go to the documentation of this file.
1 #ifndef AUXILIARY_PF_H
2 #define AUXILIARY_PF_H
3 
4 #include <array> //array
5 #include <vector> // vector
6 #include <functional> // function
7 #include <Eigen/Dense>
8 #include <cmath>
9 
10 #include "pf_base.h"
11 #include "rv_samp.h" // for k_generator
12 
13 
15 
27 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug=false>
28 class APF : public pf_base<float_t, dimy, dimx>
29 {
30 public:
31 
33  using ssv = Eigen::Matrix<float_t,dimx,1>;
35  using osv = Eigen::Matrix<float_t,dimy,1>;
37  using Mat = Eigen::Matrix<float_t,Eigen::Dynamic,Eigen::Dynamic>;
39  using arrayfloat_t = std::array<float_t, nparts>;
41  using arrayVec = std::array<ssv, nparts>;
43  using arrayUInt = std::array<unsigned int, nparts>;
45  static constexpr unsigned int num_particles = nparts;
46 
47 public:
48 
53  APF(const unsigned int &rs=1);
54 
55 
59  virtual ~APF();
60 
65  float_t getLogCondLike () const;
66 
67 
72  std::vector<Mat> getExpectations () const;
73 
74 
80  void filter(const osv &data, const std::vector<std::function<const Mat(const ssv&)> >& fs = std::vector<std::function<const Mat(const ssv&)> >());
81 
82 
88  virtual float_t logMuEv (const ssv &x1 ) = 0;
89 
90 
96  virtual ssv propMu (const ssv &xtm1 ) = 0;
97 
98 
104  virtual ssv q1Samp (const osv &y1) = 0;
105 
106 
112  virtual ssv fSamp (const ssv &xtm1) = 0;
113 
114 
121  virtual float_t logQ1Ev (const ssv &x1, const osv &y1) = 0;
122 
123 
130  virtual float_t logGEv (const osv &yt, const ssv &xt) = 0;
131 
132 
133 protected:
135  std::array<ssv,nparts> m_particles;
136 
138  std::array<float_t,nparts> m_logUnNormWeights;
139 
141  unsigned int m_now;
142 
145 
147  unsigned int m_rs;
148 
150  resamp_t m_resampler;
151 
154 
156  std::vector<Mat> m_expectations;
157 
158 };
159 
160 
161 
162 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
164  : m_now(0)
165  , m_logLastCondLike(0.0)
166  , m_rs(rs)
167 {
168  std::fill(m_logUnNormWeights.begin(), m_logUnNormWeights.end(), 0.0);
169 }
170 
171 
172 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
174 
175 
176 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
177 void APF<nparts, dimx, dimy, resamp_t, float_t, debug>::filter(const osv &data, const std::vector<std::function<const Mat(const ssv&)> >& fs)
178 {
179 
180  if(m_now > 0)
181  {
182 
183  // set up "first stage weights" to make k index sampler
184  arrayfloat_t logFirstStageUnNormWeights = m_logUnNormWeights;
185  arrayVec oldPartics = m_particles;
186  float_t m3(-std::numeric_limits<float_t>::infinity());
187  float_t m2(-std::numeric_limits<float_t>::infinity());
188  for(size_t ii = 0; ii < nparts; ++ii)
189  {
190  // update m3
191  if(m_logUnNormWeights[ii] > m3)
192  m3 = m_logUnNormWeights[ii];
193 
194  // sample
195  ssv xtm1 = oldPartics[ii];
196  logFirstStageUnNormWeights[ii] += logGEv(data, propMu(xtm1));
197 
198  // accumulate things
199  if(logFirstStageUnNormWeights[ii] > m2)
200  m2 = logFirstStageUnNormWeights[ii];
201 
202  // print stuff if debug mode is on
203  if constexpr(debug) {
204  std::cout << "time: " << m_now
205  << ", first stage log unnorm weight: " << logFirstStageUnNormWeights[ii]
206  << "\n";
207  }
208 
209  }
210 
211  // draw ks (indexes) (handles underflow issues)
212  arrayUInt myKs = m_kGen.sample(logFirstStageUnNormWeights);
213 
214  // now draw xts
215  float_t m1(-std::numeric_limits<float_t>::infinity());
216  float_t first_cll_sum(0.0);
217  float_t second_cll_sum(0.0);
218  float_t third_cll_sum(0.0);
219  ssv xtm1k;
220  ssv muT;
221  for(size_t ii = 0; ii < nparts; ++ii)
222  {
223  // calclations for log p(y_t|y_{1:t-1}) (using log-sum-exp trick)
224  second_cll_sum += std::exp( logFirstStageUnNormWeights[ii] - m2 );
225  third_cll_sum += std::exp( m_logUnNormWeights[ii] - m3 );
226 
227  // sampling and unnormalized weight update
228  xtm1k = oldPartics[myKs[ii]];
229  m_particles[ii] = fSamp(xtm1k);
230  muT = propMu(xtm1k);
231  m_logUnNormWeights[ii] += logGEv(data, m_particles[ii]) - logGEv(data, muT);
232 
233  if constexpr(debug){
234  std::cout << "time: " << m_now
235  << ", transposed sample: " << m_particles[ii].transpose()
236  << ", log unnorm weight: " << m_logUnNormWeights[ii] << "\n";
237  }
238 
239  // update m1
240  if(m_logUnNormWeights[ii] > m1)
241  m1 = m_logUnNormWeights[ii];
242  }
243 
244  // calculate estimate for log of last conditonal likelihood
245  for(size_t p = 0; p < nparts; ++p)
246  first_cll_sum += std::exp( m_logUnNormWeights[p] - m1 );
247  m_logLastCondLike = m1 + std::log(first_cll_sum) + m2 + std::log(second_cll_sum) - 2*m3 - 2*std::log(third_cll_sum);
248 
249  if constexpr(debug)
250  std::cout << "time: " << m_now << ", log cond like: " << m_logLastCondLike << "\n";
251 
252  // calculate expectations before you resample
253  unsigned int fId(0);
254  for(auto & h : fs){
255 
256  Mat testOutput = h(m_particles[0]);
257  unsigned int rows = testOutput.rows();
258  unsigned int cols = testOutput.cols();
259  Mat numer = Mat::Zero(rows,cols);
260  float_t denom(0.0);
261 
262  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
263  numer += h(m_particles[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - m1);
264  denom += std::exp(m_logUnNormWeights[prtcl] - m1);
265  }
266  m_expectations[fId] = numer/denom;
267 
268  if constexpr(debug)
269  std::cout << "transposed expectation " << fId << "; " << m_expectations[fId] << "\n";
270 
271  fId++;
272  }
273 
274  // if you have to resample
275  if( (m_now+1)%m_rs == 0)
277 
278  // advance time
279  m_now += 1;
280 
281  } else { // (m_now == 0)
282 
283  float_t max(-std::numeric_limits<float_t>::infinity());
284  for(size_t ii = 0; ii < nparts; ++ii)
285  {
286  // sample particles
287  m_particles[ii] = q1Samp(data);
289  m_logUnNormWeights[ii] += logGEv(data, m_particles[ii]);
290  m_logUnNormWeights[ii] -= logQ1Ev(m_particles[ii], data);
291 
292  // print stuff if debug mode is on
293  if constexpr(debug) {
294  std::cout << "time: " << m_now
295  << ", log unnorm weight: " << m_logUnNormWeights[ii]
296  << ", transposed sample: " << m_particles[ii].transpose()
297  << "\n";
298  }
299 
300  // update maximum
301  if( m_logUnNormWeights[ii] > max)
302  max = m_logUnNormWeights[ii];
303  }
304 
305  // calculate log-likelihood with log-exp-sum trick
306  float_t sumExp(0.0);
307  for( size_t i = 0; i < nparts; ++i){
308  sumExp += std::exp( m_logUnNormWeights[i] - max );
309  }
310  m_logLastCondLike = - std::log( static_cast<float_t>(nparts) ) + max + std::log(sumExp);
311 
312  // calculate expectations before you resample
313  m_expectations.resize(fs.size());
314  unsigned int fId(0);
315  for(auto & h : fs){
316 
317  Mat testOutput = h(m_particles[0]);
318  unsigned int rows = testOutput.rows();
319  unsigned int cols = testOutput.cols();
320  Mat numer = Mat::Zero(rows,cols);
321  float_t denom(0.0);
322  for(size_t prtcl = 0; prtcl < nparts; ++prtcl){
323  numer += h(m_particles[prtcl]) * std::exp(m_logUnNormWeights[prtcl] - max);
324  denom += std::exp(m_logUnNormWeights[prtcl] - max);
325  }
326  m_expectations[fId] = numer/denom;
327 
328  if constexpr(debug)
329  std::cout << "transposed expectation " << fId << "; " << m_expectations[fId] << "\n";
330 
331  fId++;
332  }
333 
334  // resample if you should (automatically normalizes)
335  if( (m_now+1) % m_rs == 0)
337 
338  // advance time step
339  m_now += 1;
340  }
341 
342 }
343 
344 
345 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
347 {
348  return m_logLastCondLike;
349 }
350 
351 
352 template<size_t nparts, size_t dimx, size_t dimy, typename resamp_t, typename float_t, bool debug>
354 {
355  return m_expectations;
356 }
357 
358 #endif //APF_H
359 
virtual ssv fSamp(const ssv &xtm1)=0
Samples from f.
rvsamp::k_gen< nparts, float_t > m_kGen
k generator object (default ctor&#39;d)
Definition: auxiliary_pf.h:153
A base-class for Auxiliary Particle Filtering. Filtering only, no smoothing.
Definition: auxiliary_pf.h:28
Definition: pf_base.h:19
std::array< float_t, nparts > m_logUnNormWeights
particle unnormalized weights
Definition: auxiliary_pf.h:138
std::vector< Mat > getExpectations() const
return all stored expectations (taken with respect to $p(x_t|y_{1:t})$
Definition: auxiliary_pf.h:353
std::array< unsigned int, N > sample(const std::array< float_t, N > &logWts)
sample N times from (0,1,...N-1)
Definition: rv_samp.h:842
unsigned int m_rs
the resampling schedule
Definition: auxiliary_pf.h:147
virtual float_t logGEv(const osv &yt, const ssv &xt)=0
Evaluates the log of g.
unsigned int m_now
curren time
Definition: auxiliary_pf.h:141
all rv samplers must inherit from this.
float_t m_logLastCondLike
log p(y_t|y_{1:t-1}) or log p(y1)
Definition: auxiliary_pf.h:144
virtual float_t logMuEv(const ssv &x1)=0
Evaluates the log of mu.
resamp_t m_resampler
resampler object (default ctor&#39;d)
Definition: auxiliary_pf.h:150
virtual ssv q1Samp(const osv &y1)=0
Samples from q1.
virtual ~APF()
The (virtual) destructor.
Definition: auxiliary_pf.h:173
virtual ssv propMu(const ssv &xtm1)=0
Evaluates the proposal distribution taking a Eigen::Matrix<float_t,dimx,1> from the previous time&#39;s s...
static constexpr unsigned int num_particles
Definition: auxiliary_pf.h:45
std::array< unsigned int, nparts > arrayUInt
Definition: auxiliary_pf.h:43
Eigen::Matrix< float_t, Eigen::Dynamic, Eigen::Dynamic > Mat
Definition: auxiliary_pf.h:37
std::vector< Mat > m_expectations
expectations E[h(x_t) | y_{1:t}] for user defined "h"s
Definition: auxiliary_pf.h:156
All non Rao-Blackwellized particle filters inherit from this.
APF(const unsigned int &rs=1)
The constructor.
Definition: auxiliary_pf.h:163
std::array< ssv, nparts > m_particles
particle samples
Definition: auxiliary_pf.h:135
Eigen::Matrix< float_t, dimx, 1 > ssv
Definition: auxiliary_pf.h:33
void filter(const osv &data, const std::vector< std::function< const Mat(const ssv &)> > &fs=std::vector< std::function< const Mat(const ssv &)> >())
Use a new datapoint to update the filtering distribution (or smoothing if pathLength > 0)...
Definition: auxiliary_pf.h:177
float_t getLogCondLike() const
Get the latest log conditional likelihood.
Definition: auxiliary_pf.h:346
std::array< ssv, nparts > arrayVec
Definition: auxiliary_pf.h:41
Eigen::Matrix< float_t, dimy, 1 > osv
Definition: auxiliary_pf.h:35
virtual float_t logQ1Ev(const ssv &x1, const osv &y1)=0
Evaluates the log of q1.
std::array< float_t, nparts > arrayfloat_t
Definition: auxiliary_pf.h:39