def test_orientation_dcm(self): """ Test orientation_dcm. """ test_dcm = self.move.orientation_dcm(self.time) ref_dcm = euler2dcm(self.a[0], self.a[1], self.a[2]) result = np.ma.allclose(test_dcm, ref_dcm, atol=1e-08) self.assertTrue(result)
def set_init_orientation(self, gamma, theta, psi): """ Set initial orientation of calibration table. Parameters ---------- gamma: float, rad, roll theta: float, rad, pitch psi: float, rad, yaw """ self.dcm[0] = euler2dcm(gamma, theta, psi)
def orientation_dcm(self, t): """ Calculate angle at specified time. Parameters ---------- t: float, sec, time """ a = self.turn_rate * (t - self.time_start) turn_dcm = euler2dcm(a[0], a[1], a[2]) return np.dot(self.init_dcm, turn_dcm)
def __init__(self, time1, x_turn, y_turn, z_turn ): """ Init rotation object Parameters ---------- time: float, sec x_turn: float, rad, relative turn around x axis of IMU y_turn: float, rad, relative turn around y axis of IMU z_turn: float, rad, relative turn around z axis of IMU """ # start time of current rotation self.time_start = 0. # time between start and end of current rotation self.time_total = time1 self.init_dcm = euler2dcm(0., 0., 0.) self.angle_total = np.array([x_turn, y_turn, z_turn]) self.turn_rate = self._calc_rate(self.time_total, self.angle_total)
def add(self, move): """ Add calibration table movement. Parameters ---------- move: obj, one simple rotation """ if isinstance(move, RotateIMU): move.time_start = sum(self.time) self.time.append(move.time_total) move.init_dcm = self.dcm[-1] turn_dcm = euler2dcm(move.angle_total[0], move.angle_total[1], move.angle_total[2]) self.dcm.append(np.dot(self.dcm[-1], turn_dcm)) self.sequence.append(move) else: raise TypeError('Must be instance of RotateIMU')
def set_orientation(self, roll, pitch, yaw): """ Set initial orientation. """ self._dcmbn = euler2dcm(roll, pitch, yaw)
def __init__(self): self.time = [0.] self.dcm = [euler2dcm(0., 0., 0.)] self.sequence = []