def __init__(self, n, init_pdf, p_bt_btp, kalman_args, kalman_class = KalmanFilter):
r"""Initialise marginalized particle filter.
:param int n: number of particles
:param init_pdf: probability density which initial particles are sampled from. (both
:math:`a_t` and :math:`b_t` parts)
:type init_pdf: :class:`~pybayes.pdfs.Pdf`
:param p_bt_btp: :math:`p(b_t|b_{t-1})` cpdf of the (b part of the) state in *t* given
state in *t-1*
:type p_bt_btp: :class:`~pybayes.pdfs.CPdf`
:param dict kalman_args: arguments for the Kalman filter, passed as dictionary; *state_pdf*
key should not be speficied as it is supplied by the marginalized particle filter
:param class kalman_class: class of the filter used for the :math:`a_t` part of the system;
defaults to :class:`KalmanFilter`
"""
if not isinstance(n, int) or n < 1:
raise TypeError("n must be a positive integer")
if not isinstance(init_pdf, Pdf) or not isinstance(p_bt_btp, CPdf):
raise TypeError("init_pdf must be a Pdf and p_bt_btp must be a CPdf")
if not issubclass(kalman_class, KalmanFilter):
raise TypeError("kalman_class must be a subclass (not an instance) of KalmanFilter")
b_shape = p_bt_btp.shape()
if p_bt_btp.cond_shape() != b_shape:
raise ValueError("p_bt_btp's shape ({0}) and cond shape ({1}) must both be {2}".format(
p_bt_btp.shape(), p_bt_btp.cond_shape(), b_shape))
self.p_bt_btp = p_bt_btp
a_shape = init_pdf.shape() - b_shape
# this will be removed when hardcoding Q,R into kalman filter will be removed
kalman_args['Q'] = np.array([[-123.]])
kalman_args['R'] = np.array([[-494658.]])
# generate both initial parts of particles
init_particles = init_pdf.samples(n)
# create all Kalman filters first
self.kalmans = np.empty(n, dtype=KalmanFilter) # array of references to Kalman filters
gausses = np.empty(n, dtype=GaussPdf) # array of Kalman filter state pdfs
for i in range(n):
gausses[i] = GaussPdf(init_particles[i,0:a_shape], np.array([[1.]]))
kalman_args['state_pdf'] = gausses[i]
self.kalmans[i] = kalman_class(**kalman_args)
# construct apost pdf. Important: reference to ith GaussPdf is shared between ith Kalman
# filter's state_pdf and ith memp't gauss
self.memp = MarginalizedEmpPdf(gausses, init_particles[:,a_shape:])
class MarginalizedParticleFilter(Filter):
r"""Simple marginalized particle filter implementation. Assume that tha state vector :math:`x`
can be divided into two parts :math:`x_t = (a_t, b_t)` and that the pdf representing the process
model can be factorised as follows:
.. math:: p(x_t|x_{t-1}) = p(a_t|a_{t-1}, b_t) p(b_t | b_{t-1})
and that the :math:`a_t` part (given :math:`b_t`) can be estimated with (a subbclass of) the
:class:`KalmanFilter`. Such system may be suitable for the marginalized particle filter, whose
posterior pdf takes the form
.. math::
p &= \sum_{i=1}^n \omega_i p(a_t | y_{1:t}, b_{1:t}^{(i)}) \delta(b_t - b_t^{(i)}) \\
p(a_t | y_{1:t}, b_{1:t}^{(i)}) &\text{ is posterior pdf of i}^{th} \text{ Kalman filter} \\
\text{where } \quad \quad \quad \quad \quad
b_t^{(i)} &\text{ is value of the (b part of the) i}^{th} \text{ particle} \\
\omega_i \geq 0 &\text{ is weight of the i}^{th} \text{ particle} \quad \sum \omega_i = 1
**Note:** currently :math:`b_t` is hard-coded to be process and observation noise covariance of the
:math:`a_t` part. This will be changed soon and :math:`b_t` will be passed as condition to
:meth:`KalmanFilter.bayes`.
"""
def __init__(self, n, init_pdf, p_bt_btp, kalman_args, kalman_class = KalmanFilter):
r"""Initialise marginalized particle filter.
:param int n: number of particles
:param init_pdf: probability density which initial particles are sampled from. (both
:math:`a_t` and :math:`b_t` parts)
:type init_pdf: :class:`~pybayes.pdfs.Pdf`
:param p_bt_btp: :math:`p(b_t|b_{t-1})` cpdf of the (b part of the) state in *t* given
state in *t-1*
:type p_bt_btp: :class:`~pybayes.pdfs.CPdf`
:param dict kalman_args: arguments for the Kalman filter, passed as dictionary; *state_pdf*
key should not be speficied as it is supplied by the marginalized particle filter
:param class kalman_class: class of the filter used for the :math:`a_t` part of the system;
defaults to :class:`KalmanFilter`
"""
if not isinstance(n, int) or n < 1:
raise TypeError("n must be a positive integer")
if not isinstance(init_pdf, Pdf) or not isinstance(p_bt_btp, CPdf):
raise TypeError("init_pdf must be a Pdf and p_bt_btp must be a CPdf")
if not issubclass(kalman_class, KalmanFilter):
raise TypeError("kalman_class must be a subclass (not an instance) of KalmanFilter")
b_shape = p_bt_btp.shape()
if p_bt_btp.cond_shape() != b_shape:
raise ValueError("p_bt_btp's shape ({0}) and cond shape ({1}) must both be {2}".format(
p_bt_btp.shape(), p_bt_btp.cond_shape(), b_shape))
self.p_bt_btp = p_bt_btp
a_shape = init_pdf.shape() - b_shape
# this will be removed when hardcoding Q,R into kalman filter will be removed
kalman_args['Q'] = np.array([[-123.]])
kalman_args['R'] = np.array([[-494658.]])
# generate both initial parts of particles
init_particles = init_pdf.samples(n)
# create all Kalman filters first
self.kalmans = np.empty(n, dtype=KalmanFilter) # array of references to Kalman filters
gausses = np.empty(n, dtype=GaussPdf) # array of Kalman filter state pdfs
for i in range(n):
gausses[i] = GaussPdf(init_particles[i,0:a_shape], np.array([[1.]]))
kalman_args['state_pdf'] = gausses[i]
self.kalmans[i] = kalman_class(**kalman_args)
# construct apost pdf. Important: reference to ith GaussPdf is shared between ith Kalman
# filter's state_pdf and ith memp't gauss
self.memp = MarginalizedEmpPdf(gausses, init_particles[:,a_shape:])
def __str__(self):
ret = ""
for i in range(self.kalmans.shape[0]):
ret += " {0}: {1:0<5.3f} * {2} {3} kf.S: {4}\n".format(i, self.memp.weights[i],
self.memp.gausses[i], self.memp.particles[i], self.kalmans[i].S)
return ret[:-1] # trim the last newline
def bayes(self, yt, cond = None):
r"""Perform Bayes rule for new measurement :math:`y_t`. Uses following algorithm:
1. generate new b parts of particles: :math:`b_t^{(i)} = \text{sample from }
p(b_t^{(i)}|b_{t-1}^{(i)}) \quad \forall i`
2. :math:`\text{set } Q_i = b_t^{(i)} \quad R_i = b_t^{(i)}` where :math:`Q_i, R_i` is
covariance of process (respectively observation) noise in ith Kalman filter.
3. perform Bayes rule for each Kalman filter using passed observation :math:`y_t`
4. recompute weights: :math:`\omega_i = p(y_t | y_{1:t-1}, b_t^{(i)}) \omega_i` where
:math:`p(y_t | y_{1:t-1}, b_t^{(i)})` is *evidence* (*marginal likelihood*) pdf of ith Kalman
filter.
5. normalise weights
6. resample particles
"""
for i in range(self.kalmans.shape[0]):
# generate new b_t
self.memp.particles[i] = self.p_bt_btp.sample(self.memp.particles[i])
# assign b_t to kalman filter
# TODO: more general and correct apprach would be some kind of QRKalmanFilter that would
# accept b_t in condition. This is planned in future.
kalman = self.kalmans[i]
kalman.Q[0,0] = self.memp.particles[i,0]
kalman.R[0,0] = self.memp.particles[i,0]
kalman.bayes(yt)
self.memp.weights[i] *= exp(kalman.evidence_log(yt))
# make sure that weights are normalised
self.memp.normalise_weights()
# resample particles
self._resample()
return True
def _resample(self):
indices = self.memp.get_resample_indices()
self.kalmans = self.kalmans[indices] # resample kalman filters (makes references, not hard copies)
self.memp.particles = self.memp.particles[indices] # resample particles
for i in range(self.kalmans.shape[0]):
if indices[i] == i: # copy only when needed
continue
self.kalmans[i] = deepcopy(self.kalmans[i]) # we need to deep copy ith kalman
self.memp.gausses[i] = self.kalmans[i].P # reassign reference to correct (new) state pdf
self.memp.weights[:] = 1./self.kalmans.shape[0] # set weights to 1/n
return True
def posterior(self):
return self.memp
