def test_case2(self): number_of_cycles = 50 number_to_success = 45 number_succeeded = 0 for cycle in range(number_of_cycles): number_of_iterations = random.randint(2, 100) motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)] x = task.generate_ground_truth(motions) final_robot = x[0] measurements = x[1] estimated_position = task.particle_filter(motions, measurements) if task.check_output(final_robot, estimated_position): number_succeeded += 1 print "TEST: Succeeded %d runs of %d" % (number_succeeded, number_of_cycles) if number_succeeded < number_to_success: self.assertTrue(False, "Too small succes ratio")
def test_case1(self): motions = [[2. * pi / 10, 20.] for row in range(8)] measurements = [[4.746936, 3.859782, 3.045217, 2.045506], [3.510067, 2.916300, 2.146394, 1.598332], [2.972469, 2.407489, 1.588474, 1.611094], [1.906178, 1.193329, 0.619356, 0.807930], [1.352825, 0.662233, 0.144927, 0.799090], [0.856150, 0.214590, 5.651497, 1.062401], [0.194460, 5.660382, 4.761072, 2.471682], [5.717342, 4.736780, 3.909599, 2.342536]] expected = [93.476, 75.186, 5.2664] result = task.particle_filter(motions, measurements) self.assertTrue(abs(result[0] - expected[0]) < task.tolerance_xy, "predicted X=%f is far away from expected %f" % (result[0], expected[0])) self.assertTrue(abs(result[1] - expected[1]) < task.tolerance_xy, "predicted Y=%f is far away from expected %f" % (result[1], expected[1])) self.assertTrue(abs(result[2] - expected[2]) < task.tolerance_orientation, "predicted orientation=%f is far away from expected %f" % (result[2], expected[2]))