Example #1
0
    def state_function(self, state, dt):
        result = state

        this_attitude = Quaternion(
            result[0], result[1], result[2], result[3]).normalized()
        delta = Quaternion()

        advance = Quaternion.integrate_rotation_rate(
            self.current_gyro.roll + result[6],
            self.current_gyro.pitch + result[5],
            self.current_gyro.yaw + result[4],
            dt)
        delta = delta * advance

        next_attitude = (this_attitude * delta).normalized()

        result[0] = next_attitude.w
        result[1] = next_attitude.x
        result[2] = next_attitude.y
        result[3] = next_attitude.z
        return result
Example #2
0
 def pitch_error(self, pitch):
     return (self.attitude() *
             Quaternion.from_euler(
             0., pitch, 0.).conjugated()).euler().pitch
Example #3
0
 def attitude(self):
     return Quaternion(*self.ukf.state[0:4,0]).normalized()
Example #4
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]]])
Example #5
0
 def attitude(self):
     return Quaternion.from_euler(0., self.pitch(), 0.)
Example #6
0
 def yaw_meas(state):
     this_attitude = Quaternion(
         state[0], state[1], state[2], state[3]).normalized()
     offset = this_attitude * mounting
     return numpy.array([[offset.euler().yaw]])
Example #7
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
Example #8
0
 def yaw_measurement(state):
     quaternion = Quaternion(
         state[0], state[1], state[2], state[3]).normalized()
     return numpy.array([quaternion.euler().yaw])
Example #9
0
    def accel_measurement(state):
        quaternion = Quaternion(
            state[0,0], state[1,0], state[2,0], state[3,0]).normalized()

        return AttitudeEstimator.orientation_to_accel(quaternion)