def test_to_array(self):
pose = Pose(Position(1, 2), 3)
pose_array = pose.to_array()
self.assertEqual(pose_array[0], 1)
self.assertEqual(pose_array[1], 2)
self.assertEqual(pose_array[2], 3)
self.assertTrue(type(pose_array) is np.ndarray)
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
