def test_quat_euler(self):
for i, eul in enumerate(eulers):
np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7)
np.testing.assert_allclose(quats[i],
euler2quat(quat2euler(quats[i])),
rtol=1e-6)
for i, eul in enumerate(eulers):
np.testing.assert_allclose(quats[i],
euler2quat(list(eul)),
rtol=1e-7)
np.testing.assert_allclose(quats[i],
euler2quat(quat2euler(list(quats[i]))),
rtol=1e-6)
np.testing.assert_allclose(quats, euler2quat(eulers), rtol=1e-7)
np.testing.assert_allclose(quats,
euler2quat(quat2euler(quats)),
rtol=1e-6)
def liveLocationMsg(self, time):
fix = messaging.log.LiveLocationData.new_message()
predicted_state = self.kf.x
fix_ecef = predicted_state[States.ECEF_POS]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
fix.lat = float(fix_pos_geo[0])
fix.lon = float(fix_pos_geo[1])
fix.alt = float(fix_pos_geo[2])
fix.speed = float(np.linalg.norm(
predicted_state[States.ECEF_VELOCITY]))
orientation_ned_euler = ned_euler_from_ecef(
fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION]))
fix.roll = math.degrees(orientation_ned_euler[0])
fix.pitch = math.degrees(orientation_ned_euler[1])
fix.heading = math.degrees(orientation_ned_euler[2])
fix.gyro = [
float(predicted_state[10]),
float(predicted_state[11]),
float(predicted_state[12])
]
fix.accel = [
float(predicted_state[19]),
float(predicted_state[20]),
float(predicted_state[21])
]
ned_vel = self.converter.ecef2ned(
predicted_state[States.ECEF_POS] +
predicted_state[States.ECEF_VELOCITY]) - self.converter.ecef2ned(
predicted_state[States.ECEF_POS])
fix.vNED = [float(ned_vel[0]), float(ned_vel[1]), float(ned_vel[2])]
fix.source = 'kalman'
#local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY])
#fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0]))
#fix.yawCalibration = math.degrees(math.atan2(local_vel[1], local_vel[0]))
imu_frame = predicted_state[States.IMU_OFFSET]
fix.imuFrame = [
math.degrees(imu_frame[0]),
math.degrees(imu_frame[1]),
math.degrees(imu_frame[2])
]
return fix
def liveLocationMsg(self, time):
fix = messaging.log.LiveLocationData.new_message()
predicted_state = self.kf.x
fix_ecef = predicted_state[0:3]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
fix.lat = float(fix_pos_geo[0])
fix.lon = float(fix_pos_geo[1])
fix.alt = float(fix_pos_geo[2])
fix.speed = float(np.linalg.norm(predicted_state[7:10]))
orientation_ned_euler = ned_euler_from_ecef(
fix_ecef, quat2euler(predicted_state[3:7]))
fix.roll = float(orientation_ned_euler[0] * 180 / np.pi)
fix.pitch = float(orientation_ned_euler[1] * 180 / np.pi)
fix.heading = float(orientation_ned_euler[2] * 180 / np.pi)
fix.gyro = [
float(predicted_state[10]),
float(predicted_state[11]),
float(predicted_state[12])
]
fix.accel = [
float(predicted_state[19]),
float(predicted_state[20]),
float(predicted_state[21])
]
local_vel = rotations_from_quats(predicted_state[3:7]).T.dot(
predicted_state[7:10])
fix.pitchCalibration = float(
(np.arctan2(local_vel[2], local_vel[0])) * 180 / np.pi)
fix.yawCalibration = float(
(np.arctan2(local_vel[1], local_vel[0])) * 180 / np.pi)
fix.imuFrame = [(180 / np.pi) * float(predicted_state[23]),
(180 / np.pi) * float(predicted_state[24]),
(180 / np.pi) * float(predicted_state[25])]
return fix