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