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