def test_motion_model_circular_right_0(self): ''' Move from origin (0,0,0) to the RIGHT With v = PI, on a circle with R = 2.0 meters with dT = 1 seconds for 1 cycle. x = [2, -2, pi, pi] ''' x = 0.0 y = 0.0 o = 0.0 v = 0.0 a = 0.0 s = 0.0 # R = 2 dT = 1 u = np.array([[a], [s]]) R = get_icr_radius(s) v = np.pi * R / 2 kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) x = kf.motion_model(u) self.assertLessEqual(kf.posx, R) self.assertLessEqual(kf.posy, 0) self.assertGreaterEqual(kf.posx, -R) self.assertGreaterEqual(kf.posy, -2 * R) self.assertAlmostEqual(kf.posx, R, None, None, 0.2) self.assertAlmostEqual(kf.posy, -R, None, None, 0.2) if self.debug is True: print(kf.x)
def test_motion_model_circular_loop_left_5times(self): ''' Move from origin (0,0,0) to the LEFT With v = 1, on a circle with R = 1.75 meters with dT = 1 seconds with 5 cycles ''' x = 0.0 y = 0.0 o = 0.0 v = 1.0 a = 0.0 s = 14.0 # R = 1.75 u = np.array([[a], [s]]) dT = 1.0 kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) R = get_icr_radius(s) for i in range(1, 6): x = kf.motion_model(u) # if self.debug is True: # print(kf.x) self.assertLessEqual(kf.posx, R) self.assertLessEqual(kf.posy, 2 * R) self.assertGreaterEqual(kf.posx, -R) self.assertGreaterEqual(kf.posy, 0) if self.debug is True: print(kf.x)
def test_motion_model_circular_left_4(self): ''' Move from origin (0,0,0) to the LEFT With v = 1, on a circle with R = 1.75 meters with dT = 4 seconds for 1 cycle. ''' x = 0.0 y = 0.0 o = 0.0 v = 1.0 a = 0.0 s = 14.0 # R = 1.75 dT = 4 u = np.array([[a], [s]]) kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) R = get_icr_radius(s) x = kf.motion_model(u) self.assertLessEqual(kf.posx, R) self.assertLessEqual(kf.posy, 2 * R) self.assertGreaterEqual(kf.posx, -R) self.assertGreaterEqual(kf.posy, 0) if self.debug is True: print(kf.x)
def test_motion_model_circular_right_1(self): ''' Move from origin (0,0,0) to the RIGHT With v = 1, on a circle with R = 2.0 meters with dT = 1 seconds for 1 cycle. x = Rsin(v*dT/R) = 0.47 y = Rcos(v*dT/R) = 0.94 y needs a shifting with R after that ''' x = 0.0 y = 0.0 o = 0.0 v = 1.0 a = 0.0 s = 0.0 # R = 2 dT = 1 u = np.array([[a], [s]]) kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) R = get_icr_radius(s) x = kf.motion_model(u) self.assertLessEqual(kf.posx, R) self.assertLessEqual(kf.posy, 0) self.assertGreaterEqual(kf.posx, -R) self.assertGreaterEqual(kf.posy, -2 * R) if self.debug is True: print(kf.x)
def test_st_mapping(self): ''' Test the get_icr_radius function. ''' sts = [20.5, 14.0, 7.0, 0.0, -8.0, -12.5] real = [0.98, 1.75, 0, 2.08, 0.93, 0.73] res = [] for i in sts: res.append(get_icr_radius(i)) for i in range(0, len(sts)): self.assertEqual(real[i], res[i])