def predict(self, command):
if command is None:
self.x = np.dot(self.F, self.x)
else:
conversion_m_to_mm = 1000
command = Pose(command[0], command[1],
command[2]).scale(conversion_m_to_mm)
command.position = command.position.rotate(self.x[4])
oldx = self.x.copy()
self.x = np.dot(self.F, oldx) + np.dot(self.B, command.to_array())
self.P = np.dot(np.dot(self.F, self.P), np.transpose(self.F)) + self.Q
if self.P[0][0] is np.nan:
exit(0)
if np.abs(self.x[5]) > 500:
print("A The estimate of orientation speed is reaching inf!!")
self.x[5] = 0
self.P[5][5] = 0
def test_get_set(self):
pose = Pose(Position(1, 2), 3)
self.assertTrue(hasattr(pose, 'position'))
self.assertTrue(hasattr(pose, 'orientation'))
self.assertTrue(pose.position == Position(1, 2))
self.assertTrue(pose.orientation == 3)
pose.position = Position(2, 3)
pose.orientation = 1
self.assertTrue(pose.position == Position(2, 3))
self.assertTrue(pose.orientation == 1)
self.assertEqual(pose[0], 2)
self.assertEqual(pose[1], 3)
self.assertEqual(pose[2], 1)
with self.assertRaises(IndexError):
pose[3]
