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