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