示例#1
0
 def pitch_error(self, pitch):
     return (self.attitude() *
             Quaternion.from_euler(
             0., pitch, 0.).conjugated()).euler().pitch
示例#2
0
 def attitude(self):
     return Quaternion.from_euler(0., self.pitch(), 0.)
示例#3
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]]])
示例#4
0
    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