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
def pitch_error(self, pitch):
return (self.attitude() *
Quaternion.from_euler(
0., pitch, 0.).conjugated()).euler().pitch
def attitude(self):
return Quaternion(*self.ukf.state[0:4,0]).normalized()
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 attitude(self):
return Quaternion.from_euler(0., self.pitch(), 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]])
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
def yaw_measurement(state):
quaternion = Quaternion(
state[0], state[1], state[2], state[3]).normalized()
return numpy.array([quaternion.euler().yaw])
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)
