def pitch_error(self, pitch): return (self.attitude() * Quaternion.from_euler( 0., pitch, 0.).conjugated()).euler().pitch
def attitude(self): return Quaternion.from_euler(0., self.pitch(), 0.)
def orientation_to_accel(pitch): gravity = numpy.array([0, 0, 1.0]) expected = Quaternion.from_euler(0., pitch, 0.).conjugated().rotate(gravity) return numpy.array([[expected[1]], [expected[2]]])
def accel_to_orientation(x, y, z): roll = math.atan2(-x, z) pitch = math.atan2(y, math.sqrt(x**2 + z ** 2)) quat = Quaternion.from_euler(roll, pitch, 0) return quat