Bayes++ Bayesian Filtering Classes Release 2014.5 - Copyright (c) 2003,2004,2005,2006,2011,2012,2014 Michael Stevens
SIRFlt.hpp
Go to the documentation of this file.
1#ifndef _BAYES_FILTER_SIR
2#define _BAYES_FILTER_SIR
3
4/*
5 * Bayes++ the Bayesian Filtering Library
6 * Copyright (c) 2002 Michael Stevens
7 * See accompanying Bayes++.htm for terms and conditions of use.
8 *
9 * $Id$
10 */
11
12/*
13 * Sampling Importance Resampleing Filter Scheme.
14 * Also know as a weighted Booststrap
15 *
16 * References
17 * [1] "Novel approach to nonlinear-non-Guassian Bayesian state estimation"
18 * NJ Gordon, DJ Salmond, AFM Smith IEE Proceeding-F Vol.140 No.2 April 1993
19 * [2] Building Robust Simulation-based Filter for Evolving Data Sets"
20 * J Carpenter, P Clifford, P Fearnhead Technical Report Unversity of Oxford
21 *
22 * A variety of resampling algorithms can be used for the SIR filter.
23 * There are implementations for two algorithms:
24 * standard_resample: Standard resample algorithm from [1]
25 * systematic_resample: A Simple stratified resampler from [2]
26 * A virtual 'weighted_resample' provides an standard interface to these and defaults
27 * to the standard_resample.
28 *
29 * NOTES:
30 * SIR algorithm is sensitive to random generator
31 * In particular random uniform must be [0..1) NOT [0..1]
32 * Quantisation in the random number generator must not approach the sample size.
33 * This will result in quantisation of the resampling.
34 * For example if random identically equal to 0 becomes highly probable due to quantisation
35 * this will result in the first sample being selectively draw whatever its likelihood.
36 *
37 * Numerics
38 * Resampling requires comparisons of normalised weights. These may
39 * become insignificant if Likelihoods have a large range. Resampling becomes ill conditioned
40 * for these samples.
41 */
42#include "bayesFlt.hpp"
43
44/* Filter namespace */
45namespace Bayesian_filter
46{
47
49/*
50 * Random number generators interface
51 * Helper to allow polymorthic use of random number generators
52 */
53{
54 virtual void normal(FM::DenseVec& v) = 0;
55 virtual void uniform_01(FM::DenseVec& v) = 0;
56 virtual ~SIR_random () {}
57};
58
59
61/*
62 * Importance resampler
63 * Represents a function that computes the posterior resampling from importance weights
64 * Polymorphic function object used to parameterise the resampling operation
65 */
66{
67public:
68 typedef std::vector<std::size_t> Resamples_t; // resampling counts
69
70 virtual Float resample (Resamples_t& presamples, std::size_t& uresamples, FM::DenseVec& w, SIR_random& r) const = 0;
71 /*
72 * The resampling function
73 * Weights w are proportional to the posterior Likelihood of a state
74 * Side effect
75 * w becomes a normalised cumulative sum
76 * Random draws can be made from 'r'
77 *
78 * Exceptions:
79 * bayes_filter_exception for numerical problems with weights including
80 * any w < 0, all w == 0
81 * Return
82 * lcond, smallest normalised weight, represents conditioning of resampling solution
83 * presamples: posterior resample, the number of times each sample should appear posterior baes on it weight
84 * uresample: the number of unique resamples in the posterior == number of non zero elements in presamples
85 * Preconditions
86 * wresample,w must have same size
87 */
88};
89
91// Standard resample algorithm from [1]
92{
93public:
94 Float resample (Resamples_t& presamples, std::size_t& uresamples, FM::DenseVec& w, SIR_random& r) const;
95};
96
98// Systematic resample algorithm from [2]
99{
100public:
101 Float resample (Resamples_t& presamples, std::size_t& uresamples, FM::DenseVec& w, SIR_random& r) const;
102};
103
104
105template <class Predict_model>
107/*
108 * Generalise a predict model to sampled predict model using and SIR random
109 * To instantiate template std::sqrt is required
110 */
111{
112public:
113 Sampled_general_predict_model (std::size_t x_size, std::size_t q_size, SIR_random& random_helper) :
114 Predict_model(x_size, q_size),
116 genn(random_helper),
117 xp(x_size),
118 n(q_size), rootq(q_size)
119 {
120 first_init = true;
121 }
122
123 virtual const FM::Vec& fw(const FM::Vec& x) const
124 /*
125 * Definition of sampler for additive noise model given state x
126 * Generate Gaussian correlated samples
127 * Precond: init_GqG, automatic on first use
128 */
129 {
130 if (first_init)
131 init_GqG();
132 // Predict state using supplied functional predict model
133 xp = Predict_model::f(x);
134 // Additive random noise
135 genn.normal(n); // Independent zero mean normal
136 // multiply elements by std dev
137 for (FM::DenseVec::iterator ni = n.begin(); ni != n.end(); ++ni) {
138 *ni *= rootq[ni.index()];
139 }
140 FM::noalias(xp) += FM::prod(this->G,n); // add correlated noise
141 return xp;
142 }
143
144 void init_GqG() const
145 /* initialise predict given a change to q,G
146 * Implementation: Update rootq
147 */
148 {
149 first_init = false;
150 for (FM::Vec::const_iterator qi = this->q.begin(); qi != this->q.end(); ++qi) {
151 if (*qi < 0)
152 error (Numeric_exception("Negative q in init_GqG"));
153 rootq[qi.index()] = std::sqrt(*qi);
154 }
155 }
156private:
157 SIR_random& genn;
158 mutable FM::Vec xp;
159 mutable FM::DenseVec n;
160 mutable FM::Vec rootq; // Optimisation of sqrt(q) calculation, automatic on first use
161 mutable bool first_init;
162};
163
166// Sampled predict model generalisations
167// Names a shortened to first two letters of their model properties
168
169
170
172/*
173 * Sampling Importance Resampleing Filter Scheme.
174 * Implement a general form of SIR filter
175 * Importance resampling is delayed until an update is required. The sampler used
176 * is a parameter of update to allow a wide variety of usage.
177 * A stochastic sample is defined as a sample with a unqiue stochastic history other then roughening
178 */
179{
180 friend class SIR_kalman_scheme;
181public:
182 std::size_t stochastic_samples; // Number of stochastic samples in S
183
184 SIR_scheme (std::size_t x_size, std::size_t s_size, SIR_random& random_helper);
186 // Optimise copy assignment to only copy filter state
187
188 /* Specialisations for filter algorithm */
189 void init_S ();
190
192 // Default resampling update
194 }
195
196 virtual Float update_resample (const Importance_resampler& resampler);
197 /* Update: resample particles using weights and then roughen
198 * Return: lcond
199 */
200
202 // Predict samples without noise
204 }
206 // Predict samples with noise model
207
208 void observe (Likelihood_observe_model& h, const FM::Vec& z);
209 // Weight particles using likelihood model h and z
210
211 void observe_likelihood (const FM::Vec& lw);
212 // Observation fusion directly from likelihood weights
213
214 Float rougheningK; // Current roughening value (0 implies no roughening)
215 virtual void roughen()
216 // Generalised roughening: Default to roughen_minmax
217 {
218 if (rougheningK != 0)
220 }
221
222 static void copy_resamples (FM::ColMatrix& P, const Importance_resampler::Resamples_t& presamples);
223 // Update P by selectively copying based on presamples
224
225 SIR_random& random; // Reference random number generator helper
226
227protected:
228 void roughen_minmax (FM::ColMatrix& P, Float K) const; // roughening using minmax of P distribution
230 FM::DenseVec wir; // resamping weights
231 bool wir_update; // weights have been updated requring a resampling on update
232private:
233 static const Float rougheningKinit;
234 std::size_t x_size;
235};
236
237
239/*
240 * SIR implementation of a Kalman filter
241 * Updates Kalman statistics of SIR_filter
242 * These statistics are use to provide a specialised correlated roughening procedure
243 */
244{
245public:
246 SIR_kalman_scheme (std::size_t x_size, std::size_t s_size, SIR_random& random_helper);
247
248 /* Specialisations for filter algorithm */
249
250 void init ();
251
252 void update ()
253 // Implement Kalman_filter::update identically to SIR_scheme
255 }
256
258 // Implement identically to SIR_scheme
260 }
261
262 Float update_resample (const Importance_resampler& resampler);
263 // Modified SIR_filter update implementation: update mean and covariance of sampled distribution
264
265 void update_statistics ();
266 // Update Kalman statistics without resampling
267
268 void roughen()
269 { // Specialised correlated roughening
270 if (rougheningK != 0)
272 }
273
274protected:
275 void roughen_correlated (FM::ColMatrix& P, Float K); // Roughening using covariance of P distribution
277private:
278 static Float scaled_vector_square(const FM::Vec& v, const FM::SymMatrix& S);
279 void mean();
280};
281
282
283}//namespace
284#endif
Definition bayesFlt.hpp:33
static void error(const Numeric_exception &a)
Definition bayesFlt.cpp:35
Bayesian_filter_matrix::Float Float
Definition bayesFlt.hpp:39
std::vector< std::size_t > Resamples_t
Definition SIRFlt.hpp:68
virtual Float resample(Resamples_t &presamples, std::size_t &uresamples, FM::DenseVec &w, SIR_random &r) const =0
Definition bayesFlt.hpp:489
Definition bayesException.hpp:56
Definition SIRFlt.hpp:244
void roughen()
Definition SIRFlt.hpp:268
void roughen_correlated(FM::ColMatrix &P, Float K)
Definition SIRFlt.cpp:518
void init()
Definition SIRFlt.cpp:432
void update_statistics()
Definition SIRFlt.cpp:492
Float update_resample()
Definition SIRFlt.hpp:257
Sampled_LiAd_predict_model roughen_model
Definition SIRFlt.hpp:276
void update()
Definition SIRFlt.hpp:252
Definition SIRFlt.hpp:179
Float rougheningK
Definition SIRFlt.hpp:214
std::size_t stochastic_samples
Definition SIRFlt.hpp:182
bool wir_update
Definition SIRFlt.hpp:231
void predict(Functional_predict_model &f)
Definition SIRFlt.hpp:201
SIR_scheme & operator=(const SIR_scheme &)
Definition SIRFlt.cpp:213
Float update_resample()
Definition SIRFlt.hpp:191
friend class SIR_kalman_scheme
Definition SIRFlt.hpp:180
SIR_random & random
Definition SIRFlt.hpp:225
Importance_resampler::Resamples_t resamples
Definition SIRFlt.hpp:229
void roughen_minmax(FM::ColMatrix &P, Float K) const
Definition SIRFlt.cpp:365
static void copy_resamples(FM::ColMatrix &P, const Importance_resampler::Resamples_t &presamples)
Definition SIRFlt.cpp:324
void observe(Likelihood_observe_model &h, const FM::Vec &z)
Definition SIRFlt.cpp:291
FM::DenseVec wir
Definition SIRFlt.hpp:230
void observe_likelihood(const FM::Vec &lw)
Definition SIRFlt.cpp:308
virtual void roughen()
Definition SIRFlt.hpp:215
void init_S()
Definition SIRFlt.cpp:227
Definition bayesFlt.hpp:693
virtual void predict(Functional_predict_model &f)
Definition bayesFlt.cpp:257
FM::ColMatrix S
Definition bayesFlt.hpp:643
void init_GqG() const
Definition SIRFlt.hpp:144
Sampled_general_predict_model(std::size_t x_size, std::size_t q_size, SIR_random &random_helper)
Definition SIRFlt.hpp:113
virtual const FM::Vec & fw(const FM::Vec &x) const
Definition SIRFlt.hpp:123
Definition bayesFlt.hpp:110
Definition SIRFlt.hpp:92
Float resample(Resamples_t &presamples, std::size_t &uresamples, FM::DenseVec &w, SIR_random &r) const
Definition SIRFlt.cpp:40
Float resample(Resamples_t &presamples, std::size_t &uresamples, FM::DenseVec &w, SIR_random &r) const
Definition SIRFlt.cpp:119
FMMatrix< detail::BaseColMatrix > ColMatrix
Definition uBLASmatrix.hpp:326
FMVec< detail::BaseDenseVector > DenseVec
Definition uBLASmatrix.hpp:333
FMMatrix< detail::SymMatrixWrapper< detail::BaseRowMatrix > > SymMatrix
Definition uBLASmatrix.hpp:327
FMVec< detail::BaseVector > Vec
Definition uBLASmatrix.hpp:323
BOOST_UBLAS_INLINE detail::noalias_proxy< E > noalias(ublas::matrix_expression< E > &lvalue)
Definition uBLASmatrix.hpp:85
double Float
Definition matSupSub.hpp:55
Definition bayesException.hpp:21
Sampled_general_predict_model< Linear_invertible_predict_model > Sampled_LiInAd_predict_model
Definition SIRFlt.hpp:165
Sampled_general_predict_model< Linear_predict_model > Sampled_LiAd_predict_model
Definition SIRFlt.hpp:164
Definition SIRFlt.hpp:53
virtual void normal(FM::DenseVec &v)=0
virtual void uniform_01(FM::DenseVec &v)=0
virtual ~SIR_random()
Definition SIRFlt.hpp:56