def __init__(self, length, dataList=None): self.motionModel = MotionModel() self.sensorModel = SensorModel() self.sensorModel.loadData(dataList) self._path_data_count = self.sensorModel.sampleSize self._x_max = 0.0 self._x_center = 0.0 self._particles_count = 100 self._path_length = length self.initSample(2, 10.8)
class ParticleFilter: def __init__(self, length, dataList=None): self.motionModel = MotionModel() self.sensorModel = SensorModel() self.sensorModel.loadData(dataList) self._path_data_count = self.sensorModel.sampleSize self._x_max = 0.0 self._x_center = 0.0 self._particles_count = 100 self._path_length = length self.initSample(2, 10.8) @property def particles(self): return self._particles @property def x_max(self): return self._x_max @property def x_center(self): return self._x_center @property def path_length(self): return self._path_length def accept(self, data, direction=1): isBelievable = self.sensorModel.accept(data, direction) return isBelievable def updateParticles(self, step, isBelievable): for p in self._particles: # update motion model p.pos = self.motionModel.update(p.pos, step, isBelievable) # update sensor model if isBelievable: diff, pos = self.sensorModel.evaluate(p.pos / self._path_length) fit = diff # + 40*abs(pos-p.x/path_length) p.w *= math.exp(-LAMBDA * fit) # compute x_max, x_center def computePositions(self): # compute x where w max p_w_max = self._particles[0] for p in self._particles: if p.w >= p_w_max.w: p_w_max = p self._x_max = p_w_max.pos nu = sum(p.w for p in self._particles) if nu != 0.0: self._x_center = sum(p.w / nu * p.pos for p in self._particles) else: self._x_center = 0.0 # Normalize weights def normalizeWeights(self): nu = sum(p.w for p in self._particles) if nu: for p in self._particles: p.w = p.w / nu # Initialise particles. (mode=0:random, mode=1:uniform) def initSample(self, mode=1, nearPos=0.0): uniform_weight = 1.0 / self._particles_count self._particles = [] self._iter = 0 if mode == 0: # random self._particles = Particle.create_random_particles(self._particles_count, uniform_weight) elif mode == 1: # uniform self._particles = Particle.create_uniform_particles(self._particles_count, uniform_weight) elif mode == 2: # near for _ in range(0, self._particles_count): pos = norm.rvs(nearPos, RESAMPLE_AROUND_MAX_STANDARD_DEVIATION * 3) if pos < 0.0 or pos > self._path_length: pos = nearPos new_particle = Particle(pos, uniform_weight) self._particles.append(new_particle) # Resample particles. def resampleParticles(self, uniform_percent=RESAMPLE_UNIFORM_PERCENT): # create a weighted distribution, for fast picking dist = WeightedDistribution(self._particles) uniform_weight = 1.0 / self._particles_count u_count = (int)(self._particles_count * uniform_percent * math.exp(-0.2 * self._iter)) # uniform count self._iter += 1 max_around_count = (int)(self._particles_count * RESAMPLE_AROUND_MAX_PERCENT) # max around count pick_count = self._particles_count - u_count - max_around_count # pick count from exist particles # uniform new_particles = self.create_uniform_particles(u_count, uniform_weight) # max around for i in xrange(0, max_around_count): new_particle = Particle(norm.rvs(self._x_max, RESAMPLE_AROUND_MAX_STANDARD_DEVIATION), uniform_weight) new_particles.append(new_particle) # pick from exist particles for i in xrange(0, pick_count): p = dist.pick() if p is None: # No pick b/c all totally improbable new_particle = Particle(random.uniform(0, 1) * self._path_length, uniform_weight) else: new_particle = Particle(p.pos, p.w) new_particles.append(new_particle) self._particles = new_particles # If isBelievable is Fasle, only move particles, not update weights or resample def getPredict(self, isBelievable): self.updateParticles(SPPED, isBelievable) self.computePositions() if isBelievable: self.normalizeWeights() self.resampleParticles() return self._x_max def create_random_particles(self, particles_num, w): return [Particle(random.uniform(0, 1) * self._path_length, w) for _ in xrange(0, particles_num)] def create_uniform_particles(self, particles_num, w): return [ Particle((i + random.uniform(0, 1)) / particles_num * self._path_length, w) for i in xrange(0, particles_num) ]