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
