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)
