def test_zeroWeights(self): w = [1, 0, 0, 0, 0] p = pf.generateRandomParticles(5) resampled = pf.resample(w, p) for i in resampled: print(i) self.assertEqual(i, p[0])
def test_sum(self): num = 10 p = pf.generateRandomParticles(num) measurement = p[np.random.randint(0, len(p)-1)].senseNoiseUltrasonic() weight = pf.weight(p, measurement) cumsum = np.array(weight).cumsum()[num-1] self.assertWithinx(cumsum, 1.0, 0.000000001)
def test_weighting(self): num = 5 r = pf.Robot() p = pf.generateRandomParticles(num) + [r] for i in p: print(i) measurement = r.senseNoiseUltrasonic() weight = pf.weight(p, measurement) print(weight) self.assertAismaxinList(weight[5], weight)
def test_randomPercent(self): num = 4 randPercent = [random.random()] b = (1.0 - randPercent[0]) / (num-1) restofparticles = [b for i in range(num)] w = randPercent + restofparticles p = pf.generateRandomParticles(num) count = 0 N = 1000 for i in range(N): resampled = pf.resample(w, p) count += resampled.count(p[0]) percent = count / (N * num) self.assertWithinx(percent, randPercent[0], 0.02)
def test_genRandomParticles(self):
num = 50
p = pf.generateRandomParticles(num)
minx = p[0].fieldwidth
miny = p[0].fieldheight
maxx = 0
maxy = 0
for i in p:
if i.pose['x'] < minx:
minx = i.pose['x']
if i.pose['y'] < miny:
miny = i.pose['y']
if i.pose['x'] > maxx:
maxx = i.pose['x']
if i.pose['y'] > maxy:
maxy = i.pose['y']
print("\n", "particles generated between: ", minx, "and ", maxx, " in the x dimension")
print("particles generated between: ", miny, "and ", maxy, " in the y dimension")