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 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