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