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)
