def bsi_full(model, pa, ptraj, pind, future_trajs, find, ut, yt, tt, cur_ind):
"""
Perform backward simulation by drawing particles from
the categorical distribution with weights given by
\omega_{t|T}^i = \omega_{t|t}^i*p(x_{t+1}|x^i)
Args:
- pa (ParticleApproximation): particles approximation from which to sample
- model (FFBSi): model defining probability density function
- future_trajs (array-like): trajectory estimate of {t+1:T}
- ut (array-like): inputs signal for {t:T}
- yt (array-like): measurements for {t:T}
- tt (array-like): time stamps for {t:T}
"""
M = len(find)
N = len(pa.w)
res = numpy.empty(M, dtype=int)
#pind = numpy.asarray(range(N))
for j in xrange(M):
currfind = find[j] * numpy.ones((N,), dtype=int)
p_next = model.logp_xnext_full(pa.part, ptraj, pind,
future_trajs, currfind,
ut=ut, yt=yt, tt=tt, cur_ind=cur_ind)
w = pa.w + p_next
w = w - numpy.max(w)
w_norm = numpy.exp(w)
w_norm /= numpy.sum(w_norm)
res[j] = pf.sample(w_norm, 1)
return res
def bsi_rs(model, pa, ptraj, pind, future_trajs, find, ut, yt, tt, cur_ind,
maxpdf, max_iter):
"""
Perform backward simulation by using rejection sampling to draw particles
from the categorical distribution with weights given by
\omega_{t|T}^i = \omega_{t|t}^i*p(x_{t+1}|x^i)
Args:
- pa (ParticleApproximation): particles approximation from which to sample
- model (FFBSi): model defining probability density function
- future_trajs (array-like): trajectory estimate of {t+1:T}
- ut (array-like): inputs signal for {t:T}
- yt (array-like): measurements for {t:T}
- tt (array-like): time stamps for {t:T}
- maxpdf (float): argmax p(x_{t+1:T}|x_t)
- max_iter (int): number of attempts before falling back to bsi_full
"""
M = len(find)
todo = numpy.asarray(range(M))
res = numpy.empty(M, dtype=int)
weights = numpy.copy(pa.w)
weights -= numpy.max(weights)
weights = numpy.exp(weights)
weights /= numpy.sum(weights)
for _i in xrange(max_iter):
ind = numpy.random.permutation(pf.sample(weights, len(todo)))
pn = model.logp_xnext_full(pa.part[ind],
ptraj,
pind[ind],
future_trajs,
todo,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
test = numpy.log(numpy.random.uniform(size=len(todo)))
accept = test < pn - maxpdf
res[todo[accept]] = ind[accept]
todo = todo[~accept]
if (len(todo) == 0):
return res
# TODO, is there an efficient way to store those weights
# already calculated to avoid double work, or will that
# take more time than simply evaulating them all again?
res[todo] = bsi_full(model,
pa,
ptraj,
pind,
future_trajs,
todo,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
return res
def bsi_mcmc(model, pa, ptraj, pind, future_trajs, find, ut, yt, tt, cur_ind,
R, ancestors):
"""
Perform backward simulation by using Metropolis-Hastings to draw particles
from the categorical distribution with weights given by
\omega_{t|T}^i = \omega_{t|t}^i*p(x_{t+1}|x^i)
Args:
- pa (ParticleApproximation): particles approximation from which to sample
- model (FFBSi): model defining probability density function
- future_trajs (array-like): trajectory estimate of {t+1:T}
- ut (array-like): inputs signal for {t:T}
- yt (array-like): measurements for {t:T}
- tt (array-like): time stamps for {t:T}
- R (int): number of iterations to run the markov chain
- ancestor (array-like): ancestor of each particle from the particle filter
"""
# Perform backward simulation using an MCMC sampler proposing new
# backward particles, initialized with the filtered trajectory
M = len(find)
ind = ancestors
weights = numpy.copy(pa.w)
weights -= numpy.max(weights)
weights = numpy.exp(weights)
weights /= numpy.sum(weights)
pcurr = model.logp_xnext_full(pa.part[ind],
ptraj,
pind[ind],
future_trajs,
find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
for _j in xrange(R):
propind = numpy.random.permutation(pf.sample(weights, M))
pprop = model.logp_xnext_full(pa.part[propind],
ptraj,
pind[propind],
future_trajs,
find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
diff = pprop - pcurr
diff[diff > 0.0] = 0.0
test = numpy.log(numpy.random.uniform(size=M))
accept = test < diff
ind[accept] = propind[accept]
pcurr[accept] = pprop[accept]
return ind
def perform_ancestors_int(self, pt, M):
"""
Create smoothed trajectories by taking the forward trajectories, don't
perform post processing
Args:
- pt (ParticleTrajectory): forward trajetories
- M (int): number of trajectories to createa
"""
tmp = numpy.copy(pt[-1].pa.w)
tmp -= numpy.max(tmp)
tmp = numpy.exp(tmp)
tmp = tmp / numpy.sum(tmp)
ind = pf.sample(tmp, M)
return self.calculate_ancestors(pt, ind)
def perform_ancestors_int(self, pt, M):
"""
Create smoothed trajectories by taking the forward trajectories, don't
perform post processing
Args:
- pt (ParticleTrajectory): forward trajetories
- M (int): number of trajectories to createa
"""
tmp = numpy.copy(pt[-1].pa.w)
tmp -= numpy.max(tmp)
tmp = numpy.exp(tmp)
tmp = tmp / numpy.sum(tmp)
ind = pf.sample(tmp, M)
return self.calculate_ancestors(pt, ind)
def bsi_rs(model, pa, ptraj, pind, future_trajs, find, ut, yt, tt, cur_ind, maxpdf, max_iter):
"""
Perform backward simulation by using rejection sampling to draw particles
from the categorical distribution with weights given by
\omega_{t|T}^i = \omega_{t|t}^i*p(x_{t+1}|x^i)
Args:
- pa (ParticleApproximation): particles approximation from which to sample
- model (FFBSi): model defining probability density function
- future_trajs (array-like): trajectory estimate of {t+1:T}
- ut (array-like): inputs signal for {t:T}
- yt (array-like): measurements for {t:T}
- tt (array-like): time stamps for {t:T}
- maxpdf (float): argmax p(x_{t+1:T}|x_t)
- max_iter (int): number of attempts before falling back to bsi_full
"""
M = len(find)
todo = numpy.asarray(range(M))
res = numpy.empty(M, dtype=int)
weights = numpy.copy(pa.w)
weights -= numpy.max(weights)
weights = numpy.exp(weights)
weights /= numpy.sum(weights)
for _i in xrange(max_iter):
ind = numpy.random.permutation(pf.sample(weights, len(todo)))
pn = model.logp_xnext_full(pa.part[ind], ptraj, pind[ind],
future_trajs, todo,
ut=ut, yt=yt, tt=tt, cur_ind=cur_ind)
test = numpy.log(numpy.random.uniform(size=len(todo)))
accept = test < pn - maxpdf
res[todo[accept]] = ind[accept]
todo = todo[~accept]
if (len(todo) == 0):
return res
# TODO, is there an efficient way to store those weights
# already calculated to avoid double work, or will that
# take more time than simply evaulating them all again?
res[todo] = bsi_full(model, pa, ptraj, pind, future_trajs, todo, ut=ut, yt=yt, tt=tt, cur_ind=cur_ind)
return res
def bsi_mcmc(model, pa, ptraj, pind, future_trajs, find, ut, yt, tt, cur_ind, R, ancestors):
"""
Perform backward simulation by using Metropolis-Hastings to draw particles
from the categorical distribution with weights given by
\omega_{t|T}^i = \omega_{t|t}^i*p(x_{t+1}|x^i)
Args:
- pa (ParticleApproximation): particles approximation from which to sample
- model (FFBSi): model defining probability density function
- future_trajs (array-like): trajectory estimate of {t+1:T}
- ut (array-like): inputs signal for {t:T}
- yt (array-like): measurements for {t:T}
- tt (array-like): time stamps for {t:T}
- R (int): number of iterations to run the markov chain
- ancestor (array-like): ancestor of each particle from the particle filter
"""
# Perform backward simulation using an MCMC sampler proposing new
# backward particles, initialized with the filtered trajectory
M = len(find)
ind = ancestors
weights = numpy.copy(pa.w)
weights -= numpy.max(weights)
weights = numpy.exp(weights)
weights /= numpy.sum(weights)
pcurr = model.logp_xnext_full(pa.part[ind], ptraj, pind[ind],
future_trajs, find,
ut=ut, yt=yt, tt=tt, cur_ind=cur_ind)
for _j in xrange(R):
propind = numpy.random.permutation(pf.sample(weights, M))
pprop = model.logp_xnext_full(pa.part[propind], ptraj, pind[propind],
future_trajs, find,
ut=ut, yt=yt, tt=tt, cur_ind=cur_ind)
diff = pprop - pcurr
diff[diff > 0.0] = 0.0
test = numpy.log(numpy.random.uniform(size=M))
accept = test < diff
ind[accept] = propind[accept]
pcurr[accept] = pprop[accept]
return ind
def bsi_full(model, pa, ptraj, pind, future_trajs, find, ut, yt, tt, cur_ind):
"""
Perform backward simulation by drawing particles from
the categorical distribution with weights given by
\omega_{t|T}^i = \omega_{t|t}^i*p(x_{t+1}|x^i)
Args:
- pa (ParticleApproximation): particles approximation from which to sample
- model (FFBSi): model defining probability density function
- future_trajs (array-like): trajectory estimate of {t+1:T}
- ut (array-like): inputs signal for {t:T}
- yt (array-like): measurements for {t:T}
- tt (array-like): time stamps for {t:T}
"""
M = len(find)
N = len(pa.w)
res = numpy.empty(M, dtype=int)
#pind = numpy.asarray(range(N))
for j in xrange(M):
currfind = find[j] * numpy.ones((N, ), dtype=int)
p_next = model.logp_xnext_full(pa.part,
ptraj,
pind,
future_trajs,
currfind,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
w = pa.w + p_next
w = w - numpy.max(w)
w_norm = numpy.exp(w)
w_norm /= numpy.sum(w_norm)
res[j] = pf.sample(w_norm, 1)
return res
def bsi_rsas(model, pa, ptraj, pind, future_trajs, find, ut, yt, tt, cur_ind, maxpdf, x1, P1, sv, sw, ratio):
"""
Perform backward simulation by using rejection sampling to draw particles
from the categorical distribution with weights given by
\omega_{t|T}^i = \omega_{t|t}^i*p(x_{t+1}|x^i)
Adaptively determine when to to fallback to bsi_full by using a Kalman
filter to track the prediceted acceptance rate of the rejection sampler
Based on "Adaptive Stopping for Fast Particle Smoothing" by
Taghavi, Lindsten, Svensson and Sch\"{o}n. See orignal article for details
about the meaning of the Kalman filter paramters
Args:
- pa (ParticleApproximation): particles approximation from which to sample
- model (FFBSi): model defining probability density function
- future_trajs (array-like): trajectory estimate of {t+1:T}
- ut (array-like): inputs signal for {t:T}
- yt (array-like): measurements for {t:T}
- tt (array-like): time stamps for {t:T}
- maxpdf (float): argmax p(x_{t+1:T}|x_t)
- x1 (float): initial state of Kalman filter
- P1 (float): initial covariance of Kalman filter estimate
- sv (float): process noise (for Kalman filter)
- sw (float): measurement noise (for Kalman filter)
- ratio (float): cost ration of running rejection sampling compared to
switching to the full bsi (D_0 / D_1)
"""
M = len(find)
todo = numpy.asarray(range(M))
res = numpy.empty(M, dtype=int)
weights = numpy.copy(pa.w)
weights -= numpy.max(weights)
weights = numpy.exp(weights)
weights /= numpy.sum(weights)
pk = x1
Pk = P1
stop_criteria = ratio / len(pa)
while (True):
ind = numpy.random.permutation(pf.sample(weights, len(todo)))
pn = model.logp_xnext_full(pa.part[ind], ptraj, pind[ind],
future_trajs, todo,
ut=ut, yt=yt, tt=tt, cur_ind=cur_ind)
test = numpy.log(numpy.random.uniform(size=len(todo)))
accept = test < pn - maxpdf
ak = numpy.sum(accept)
mk = len(todo)
res[todo[accept]] = ind[accept]
todo = todo[~accept]
if (len(todo) == 0):
return res
# meas update for adaptive stop
mk2 = mk * mk
sw2 = sw * sw
pk = pk + (mk * Pk) / (mk2 * Pk + sw2) * (ak - mk * pk)
Pk = (1 - (mk2 * Pk) / (mk2 * Pk + sw2)) * Pk
# predict
pk = (1 - ak / mk) * pk
Pk = (1 - ak / mk) ** 2 * Pk + sv * sv
if (pk < stop_criteria):
break
res[todo] = bsi_full(model, pa, ptraj, pind, future_trajs, todo, ut=ut, yt=yt, tt=tt, cur_ind=cur_ind)
return res
def perform_mhbp(self, pt, M, R, reduced=False):
"""
Create smoothed trajectories using Metropolis-Hastings Backward Propeser
Args:
- pt (ParticleTrajectory): forward trajetories
- M (int): number of trajectories to createa
- R (int): Number of proposal for each time step
"""
T = len(pt)
ut = self.u
yt = self.y
tt = self.t
straj = numpy.empty((T,), dtype=object)
# Initialise from end time estimates
tmp = numpy.copy(pt[-1].pa.w)
tmp -= numpy.max(tmp)
tmp = numpy.exp(tmp)
tmp = tmp / numpy.sum(tmp)
cind = pf.sample(tmp, M)
find = numpy.arange(M, dtype=int)
# anc = pt[-1].ancestors[cind]
# last_part = self.model.sample_smooth(part=pt[-1].pa.part[cind],
# ptraj=pt[:-1],
# anc=anc,
# future_trajs=None,
# find=find,
# ut=ut, yt=yt, tt=tt,
# cur_ind=T - 1)
for t in reversed(xrange(T)):
# Initialise from filtered estimate
if (t < T - 1):
ft = straj[(t + 1):]
else:
ft = None
# Initialize with filterted estimates
pnew = pt[t].pa.part[cind]
if (t > 0):
anc = pt[t].ancestors[cind]
tmp = numpy.copy(pt[t - 1].pa.w)
tmp -= numpy.max(tmp)
tmp = numpy.exp(tmp)
tmp = tmp / numpy.sum(tmp)
ptraj = pt[:t]
else:
ptraj = None
for _ in xrange(R):
if (t > 0):
# Propose new ancestors
panc = pf.sample(tmp, M)
(pnew, acc) = mc_step(model=self.model,
part=pnew,
ptraj=ptraj,
pind_prop=panc,
pind_curr=anc,
future_trajs=ft,
find=find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=t,
reduced=reduced)
anc[acc] = panc[acc]
fpart = self.model.sample_smooth(part=pnew,
ptraj=ptraj,
anc=anc,
future_trajs=ft,
find=find,
ut=ut, yt=yt, tt=tt,
cur_ind=t)
straj[t] = TrajectoryStep(ParticleApproximation(fpart))
cind = anc
self.traj = straj
if hasattr(self.model, 'post_smoothing'):
# Do e.g. constrained smoothing for RBPS models
self.traj = self.model.post_smoothing(self)
def perform_bsi(self, pt, M, method, options):
"""
Create smoothed trajectories using Backward Simulation
Args:
- pt (ParticleTrajectory): forward trajetories
- M (int): number of trajectories to createa
- method (string): Type of backward simulation to use
- optiones (dict): Parameters to the backward simulator
"""
# Sample from end time estimates
tmp = numpy.copy(pt[-1].pa.w)
tmp -= numpy.max(tmp)
tmp = numpy.exp(tmp)
tmp = tmp / numpy.sum(tmp)
ind = pf.sample(tmp, M)
ancestors = pt[-1].ancestors[ind]
last_part = self.model.sample_smooth(part=pt[-1].pa.part[ind],
ptraj=pt[:-1], anc=ancestors,
future_trajs=None, find=None,
ut=self.u, yt=self.y,
tt=self.t, cur_ind=len(pt) - 1)
self.traj = numpy.empty((len(pt),), dtype=object)
self.traj[-1] = TrajectoryStep(ParticleApproximation(last_part),
numpy.arange(M, dtype=int))
if (method == 'full'):
pass
elif (method == 'mcmc' or method == 'ancestor' or method == 'mhips'):
pass
elif (method == 'rs'):
max_iter = options['R']
elif (method == 'rsas'):
x1 = options['x1']
P1 = options['P1']
sv = options['sv']
sw = options['sw']
ratio = options['ratio']
else:
raise ValueError('Unknown sampler: %s' % method)
find = numpy.arange(M, dtype=numpy.int)
for cur_ind in reversed(xrange(len(pt) - 1)):
ft = self.traj[(cur_ind + 1):]
ut = self.u
yt = self.y
tt = self.t
if (method == 'rs'):
ind = bsi_rs(self.model, pt[cur_ind].pa,
pt[:cur_ind], pt[cur_ind].ancestors,
ft, find,
ut=ut, yt=yt, tt=tt, cur_ind=cur_ind,
maxpdf=options['maxpdf'][cur_ind],
max_iter=int(max_iter))
elif (method == 'rsas'):
ind = bsi_rsas(self.model, pt[cur_ind].pa,
pt[:cur_ind], pt[cur_ind].ancestors,
ft, find,
ut=ut, yt=yt, tt=tt, cur_ind=cur_ind,
maxpdf=options['maxpdf'][cur_ind], x1=x1,
P1=P1, sv=sv, sw=sw, ratio=ratio)
elif (method == 'mcmc'):
ind = bsi_mcmc(self.model, pt[cur_ind].pa,
pt[:cur_ind], pt[cur_ind].ancestors,
ft, find,
ut=ut, yt=yt, tt=tt, cur_ind=cur_ind,
R=options['R'], ancestors=ancestors)
ancestors = pt[cur_ind].ancestors[ind]
elif (method == 'full'):
ind = bsi_full(self.model, pt[cur_ind].pa,
pt[:cur_ind], pt[cur_ind].ancestors,
ft, find,
ut=ut, yt=yt, tt=tt, cur_ind=cur_ind)
elif (method == 'ancestor'):
ind = ancestors
ancestors = pt[cur_ind].ancestors[ind]
# Select 'previous' particle
find = numpy.arange(M, dtype=int)
tmp = self.model.sample_smooth(part=pt[cur_ind].pa.part[ind],
ptraj=pt[:cur_ind],
anc=ancestors,
future_trajs=ft,
find=find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
self.traj[cur_ind] = TrajectoryStep(ParticleApproximation(tmp),
numpy.arange(M, dtype=int))
def perform_mhbp(self, pt, M, R, reduced=False):
"""
Create smoothed trajectories using Metropolis-Hastings Backward Propeser
Args:
- pt (ParticleTrajectory): forward trajetories
- M (int): number of trajectories to createa
- R (int): Number of proposal for each time step
"""
T = len(pt)
ut = self.u
yt = self.y
tt = self.t
straj = numpy.empty((T, ), dtype=object)
# Initialise from end time estimates
tmp = numpy.copy(pt[-1].pa.w)
tmp -= numpy.max(tmp)
tmp = numpy.exp(tmp)
tmp = tmp / numpy.sum(tmp)
cind = pf.sample(tmp, M)
find = numpy.arange(M, dtype=int)
# anc = pt[-1].ancestors[cind]
# last_part = self.model.sample_smooth(part=pt[-1].pa.part[cind],
# ptraj=pt[:-1],
# anc=anc,
# future_trajs=None,
# find=find,
# ut=ut, yt=yt, tt=tt,
# cur_ind=T - 1)
for t in reversed(xrange(T)):
# Initialise from filtered estimate
if (t < T - 1):
ft = straj[(t + 1):]
else:
ft = None
# Initialize with filterted estimates
pnew = pt[t].pa.part[cind]
if (t > 0):
anc = pt[t].ancestors[cind]
tmp = numpy.copy(pt[t - 1].pa.w)
tmp -= numpy.max(tmp)
tmp = numpy.exp(tmp)
tmp = tmp / numpy.sum(tmp)
ptraj = pt[:t]
else:
ptraj = None
for _ in xrange(R):
if (t > 0):
# Propose new ancestors
panc = pf.sample(tmp, M)
(pnew, acc) = mc_step(model=self.model,
part=pnew,
ptraj=ptraj,
pind_prop=panc,
pind_curr=anc,
future_trajs=ft,
find=find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=t,
reduced=reduced)
anc[acc] = panc[acc]
fpart = self.model.sample_smooth(part=pnew,
ptraj=ptraj,
anc=anc,
future_trajs=ft,
find=find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=t)
straj[t] = TrajectoryStep(ParticleApproximation(fpart))
cind = anc
self.traj = straj
if hasattr(self.model, 'post_smoothing'):
# Do e.g. constrained smoothing for RBPS models
self.traj = self.model.post_smoothing(self)
def perform_bsi(self, pt, M, method, options):
"""
Create smoothed trajectories using Backward Simulation
Args:
- pt (ParticleTrajectory): forward trajetories
- M (int): number of trajectories to createa
- method (string): Type of backward simulation to use
- optiones (dict): Parameters to the backward simulator
"""
# Sample from end time estimates
tmp = numpy.copy(pt[-1].pa.w)
tmp -= numpy.max(tmp)
tmp = numpy.exp(tmp)
tmp = tmp / numpy.sum(tmp)
ind = pf.sample(tmp, M)
ancestors = pt[-1].ancestors[ind]
last_part = self.model.sample_smooth(part=pt[-1].pa.part[ind],
ptraj=pt[:-1],
anc=ancestors,
future_trajs=None,
find=None,
ut=self.u,
yt=self.y,
tt=self.t,
cur_ind=len(pt) - 1)
self.traj = numpy.empty((len(pt), ), dtype=object)
self.traj[-1] = TrajectoryStep(ParticleApproximation(last_part),
numpy.arange(M, dtype=int))
if (method == 'full'):
pass
elif (method == 'mcmc' or method == 'ancestor' or method == 'mhips'):
pass
elif (method == 'rs'):
max_iter = options['R']
elif (method == 'rsas'):
x1 = options['x1']
P1 = options['P1']
sv = options['sv']
sw = options['sw']
ratio = options['ratio']
else:
raise ValueError('Unknown sampler: %s' % method)
find = numpy.arange(M, dtype=numpy.int)
for cur_ind in reversed(xrange(len(pt) - 1)):
ft = self.traj[(cur_ind + 1):]
ut = self.u
yt = self.y
tt = self.t
if (method == 'rs'):
ind = bsi_rs(self.model,
pt[cur_ind].pa,
pt[:cur_ind],
pt[cur_ind].ancestors,
ft,
find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind,
maxpdf=options['maxpdf'][cur_ind],
max_iter=int(max_iter))
elif (method == 'rsas'):
ind = bsi_rsas(self.model,
pt[cur_ind].pa,
pt[:cur_ind],
pt[cur_ind].ancestors,
ft,
find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind,
maxpdf=options['maxpdf'][cur_ind],
x1=x1,
P1=P1,
sv=sv,
sw=sw,
ratio=ratio)
elif (method == 'mcmc'):
ind = bsi_mcmc(self.model,
pt[cur_ind].pa,
pt[:cur_ind],
pt[cur_ind].ancestors,
ft,
find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind,
R=options['R'],
ancestors=ancestors)
ancestors = pt[cur_ind].ancestors[ind]
elif (method == 'full'):
ind = bsi_full(self.model,
pt[cur_ind].pa,
pt[:cur_ind],
pt[cur_ind].ancestors,
ft,
find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
elif (method == 'ancestor'):
ind = ancestors
ancestors = pt[cur_ind].ancestors[ind]
# Select 'previous' particle
find = numpy.arange(M, dtype=int)
tmp = self.model.sample_smooth(part=pt[cur_ind].pa.part[ind],
ptraj=pt[:cur_ind],
anc=ancestors,
future_trajs=ft,
find=find,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
self.traj[cur_ind] = TrajectoryStep(ParticleApproximation(tmp),
numpy.arange(M, dtype=int))
def bsi_rsas(model, pa, ptraj, pind, future_trajs, find, ut, yt, tt, cur_ind,
maxpdf, x1, P1, sv, sw, ratio):
"""
Perform backward simulation by using rejection sampling to draw particles
from the categorical distribution with weights given by
\omega_{t|T}^i = \omega_{t|t}^i*p(x_{t+1}|x^i)
Adaptively determine when to to fallback to bsi_full by using a Kalman
filter to track the prediceted acceptance rate of the rejection sampler
Based on "Adaptive Stopping for Fast Particle Smoothing" by
Taghavi, Lindsten, Svensson and Sch\"{o}n. See orignal article for details
about the meaning of the Kalman filter paramters
Args:
- pa (ParticleApproximation): particles approximation from which to sample
- model (FFBSi): model defining probability density function
- future_trajs (array-like): trajectory estimate of {t+1:T}
- ut (array-like): inputs signal for {t:T}
- yt (array-like): measurements for {t:T}
- tt (array-like): time stamps for {t:T}
- maxpdf (float): argmax p(x_{t+1:T}|x_t)
- x1 (float): initial state of Kalman filter
- P1 (float): initial covariance of Kalman filter estimate
- sv (float): process noise (for Kalman filter)
- sw (float): measurement noise (for Kalman filter)
- ratio (float): cost ration of running rejection sampling compared to
switching to the full bsi (D_0 / D_1)
"""
M = len(find)
todo = numpy.asarray(range(M))
res = numpy.empty(M, dtype=int)
weights = numpy.copy(pa.w)
weights -= numpy.max(weights)
weights = numpy.exp(weights)
weights /= numpy.sum(weights)
pk = x1
Pk = P1
stop_criteria = ratio / len(pa)
while (True):
ind = numpy.random.permutation(pf.sample(weights, len(todo)))
pn = model.logp_xnext_full(pa.part[ind],
ptraj,
pind[ind],
future_trajs,
todo,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
test = numpy.log(numpy.random.uniform(size=len(todo)))
accept = test < pn - maxpdf
ak = numpy.sum(accept)
mk = len(todo)
res[todo[accept]] = ind[accept]
todo = todo[~accept]
if (len(todo) == 0):
return res
# meas update for adaptive stop
mk2 = mk * mk
sw2 = sw * sw
pk = pk + (mk * Pk) / (mk2 * Pk + sw2) * (ak - mk * pk)
Pk = (1 - (mk2 * Pk) / (mk2 * Pk + sw2)) * Pk
# predict
pk = (1 - ak / mk) * pk
Pk = (1 - ak / mk)**2 * Pk + sv * sv
if (pk < stop_criteria):
break
res[todo] = bsi_full(model,
pa,
ptraj,
pind,
future_trajs,
todo,
ut=ut,
yt=yt,
tt=tt,
cur_ind=cur_ind)
return res