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_motion_model_straight(self): ''' Check motion model for straight forward movement. ''' x = 0.0 y = 0.0 o = 0.0 v = 0.1 a = 0.0 # 7.0 means straight forward s = 7.0 dT = 1 u = np.array([[a], [s]]) kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT) x = kf.motion_model(u) self.assertEqual(kf.posx, 0.1) self.assertEqual(kf.posy, 0.0) self.assertEqual(kf.yaw, o) self.assertEqual(kf.vel, v)