def test_euler_ned(self):
for i in range(len(eulers)):
np.testing.assert_allclose(ned_eulers[i],
ned_euler_from_ecef(
ecef_positions[i], eulers[i]),
rtol=1e-7)
#np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7)
np.testing.assert_allclose(ned_eulers,
ned_euler_from_ecef(ecef_positions, eulers),
rtol=1e-7)
def handle_gps(self, current_time, log):
# ignore the message if the fix is invalid
if log.flags % 2 == 0:
return
self.last_gps_fix = current_time
self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
ecef_pos = self.converter.ned2ecef([0, 0, 0])
ecef_vel = self.converter.ned2ecef(np.array(log.vNED)) - ecef_pos
ecef_pos_R = np.diag([(3*log.verticalAccuracy)**2]*3)
ecef_vel_R = np.diag([(log.speedAccuracy)**2]*3)
#self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3))
self.unix_timestamp_millis = log.timestamp
gps_est_error = np.sqrt((self.kf.x[0] - ecef_pos[0])**2 +
(self.kf.x[1] - ecef_pos[1])**2 +
(self.kf.x[2] - ecef_pos[2])**2)
orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION])
orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef)
orientation_ned_gps = np.array([0, 0, np.radians(log.bearing)])
orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi
if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1:
cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset")
initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
self.reset_kalman(init_orient=initial_pose_ecef_quat)
self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat)
elif gps_est_error > 50:
cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset")
self.reset_kalman()
self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R)
self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R)
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
def msg_from_state(converter, calib_from_device, H, predicted_state,
predicted_cov):
predicted_std = np.sqrt(np.diagonal(predicted_cov))
fix_ecef = predicted_state[States.ECEF_POS]
fix_ecef_std = predicted_std[States.ECEF_POS_ERR]
vel_ecef = predicted_state[States.ECEF_VELOCITY]
vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
#fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
orientation_ecef = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION])
orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
device_from_ecef = rot_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
calibrated_orientation_ecef = euler_from_rot(
calib_from_device.dot(device_from_ecef))
acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION])
acc_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(
predicted_cov[States.ACCELERATION_ERR,
States.ACCELERATION_ERR]).dot(
calib_from_device.T)))
ang_vel_calib = calib_from_device.dot(
predicted_state[States.ANGULAR_VELOCITY])
ang_vel_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(
predicted_cov[States.ANGULAR_VELOCITY_ERR,
States.ANGULAR_VELOCITY_ERR]).dot(
calib_from_device.T)))
vel_device = device_from_ecef.dot(vel_ecef)
device_from_ecef_eul = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + \
list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop))
condensed_cov = predicted_cov[idxs][:, idxs]
HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef])))
vel_device_cov = HH.dot(condensed_cov).dot(HH.T)
vel_device_std = np.sqrt(np.diagonal(vel_device_cov))
vel_calib = calib_from_device.dot(vel_device)
vel_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(vel_device_cov).dot(
calib_from_device.T)))
orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
#orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
ned_vel = converter.ecef2ned(fix_ecef +
vel_ecef) - converter.ecef2ned(fix_ecef)
#ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
fix = messaging.log.LiveLocationKalman.new_message()
# write measurements to msg
measurements = [
# measurement field, value, std, valid
(fix.positionGeodetic, fix_pos_geo, np.nan * np.zeros(3), True),
(fix.positionECEF, fix_ecef, fix_ecef_std, True),
(fix.velocityECEF, vel_ecef, vel_ecef_std, True),
(fix.velocityNED, ned_vel, np.nan * np.zeros(3), True),
(fix.velocityDevice, vel_device, vel_device_std, True),
(fix.accelerationDevice, predicted_state[States.ACCELERATION],
predicted_std[States.ACCELERATION_ERR], True),
(fix.orientationECEF, orientation_ecef, orientation_ecef_std,
True),
(fix.calibratedOrientationECEF, calibrated_orientation_ecef,
np.nan * np.zeros(3), True),
(fix.orientationNED, orientation_ned, np.nan * np.zeros(3), True),
(fix.angularVelocityDevice,
predicted_state[States.ANGULAR_VELOCITY],
predicted_std[States.ANGULAR_VELOCITY_ERR], True),
(fix.velocityCalibrated, vel_calib, vel_calib_std, True),
(fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std,
True),
(fix.accelerationCalibrated, acc_calib, acc_calib_std, True),
]
for field, value, std, valid in measurements:
# TODO: can we write the lists faster?
field.value = to_float(value)
field.std = to_float(std)
field.valid = valid
return fix
def msg_from_state(converter, calib_from_device, H, predicted_state,
predicted_cov):
predicted_std = np.sqrt(np.diagonal(predicted_cov))
fix_ecef = predicted_state[States.ECEF_POS]
fix_ecef_std = predicted_std[States.ECEF_POS_ERR]
vel_ecef = predicted_state[States.ECEF_VELOCITY]
vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
#fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
orientation_ecef = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION])
orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION])
acc_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(
predicted_cov[States.ACCELERATION_ERR,
States.ACCELERATION_ERR]).dot(
calib_from_device.T)))
ang_vel_calib = calib_from_device.dot(
predicted_state[States.ANGULAR_VELOCITY])
ang_vel_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(
predicted_cov[States.ANGULAR_VELOCITY_ERR,
States.ANGULAR_VELOCITY_ERR]).dot(
calib_from_device.T)))
device_from_ecef = rot_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
vel_device = device_from_ecef.dot(vel_ecef)
device_from_ecef_eul = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
idxs = list(
range(States.ECEF_ORIENTATION_ERR.start,
States.ECEF_ORIENTATION_ERR.stop)) + list(
range(States.ECEF_VELOCITY_ERR.start,
States.ECEF_VELOCITY_ERR.stop))
condensed_cov = predicted_cov[idxs][:, idxs]
HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef])))
vel_device_cov = HH.dot(condensed_cov).dot(HH.T)
vel_device_std = np.sqrt(np.diagonal(vel_device_cov))
vel_calib = calib_from_device.dot(vel_device)
vel_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(vel_device_cov).dot(
calib_from_device.T)))
orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
#orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
ned_vel = converter.ecef2ned(fix_ecef +
vel_ecef) - converter.ecef2ned(fix_ecef)
#ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
fix = messaging.log.LiveLocationKalman.new_message()
fix.positionGeodetic.value = to_float(fix_pos_geo)
#fix.positionGeodetic.std = to_float(fix_pos_geo_std)
#fix.positionGeodetic.valid = True
fix.positionECEF.value = to_float(fix_ecef)
fix.positionECEF.std = to_float(fix_ecef_std)
fix.positionECEF.valid = True
fix.velocityECEF.value = to_float(vel_ecef)
fix.velocityECEF.std = to_float(vel_ecef_std)
fix.velocityECEF.valid = True
fix.velocityNED.value = to_float(ned_vel)
#fix.velocityNED.std = to_float(ned_vel_std)
#fix.velocityNED.valid = True
fix.velocityDevice.value = to_float(vel_device)
fix.velocityDevice.std = to_float(vel_device_std)
fix.velocityDevice.valid = True
fix.accelerationDevice.value = to_float(
predicted_state[States.ACCELERATION])
fix.accelerationDevice.std = to_float(
predicted_std[States.ACCELERATION_ERR])
fix.accelerationDevice.valid = True
fix.orientationECEF.value = to_float(orientation_ecef)
fix.orientationECEF.std = to_float(orientation_ecef_std)
fix.orientationECEF.valid = True
fix.orientationNED.value = to_float(orientation_ned)
#fix.orientationNED.std = to_float(orientation_ned_std)
#fix.orientationNED.valid = True
fix.angularVelocityDevice.value = to_float(
predicted_state[States.ANGULAR_VELOCITY])
fix.angularVelocityDevice.std = to_float(
predicted_std[States.ANGULAR_VELOCITY_ERR])
fix.angularVelocityDevice.valid = True
fix.velocityCalibrated.value = to_float(vel_calib)
fix.velocityCalibrated.std = to_float(vel_calib_std)
fix.velocityCalibrated.valid = True
fix.angularVelocityCalibrated.value = to_float(ang_vel_calib)
fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std)
fix.angularVelocityCalibrated.valid = True
fix.accelerationCalibrated.value = to_float(acc_calib)
fix.accelerationCalibrated.std = to_float(acc_calib_std)
fix.accelerationCalibrated.valid = True
return fix
def liveLocationMsg(self, time):
predicted_state = self.kf.x
predicted_cov = self.kf.P
predicted_std = np.sqrt(np.diagonal(self.kf.P))
fix_ecef = predicted_state[States.ECEF_POS]
fix_ecef_std = predicted_std[States.ECEF_POS_ERR]
vel_ecef = predicted_state[States.ECEF_VELOCITY]
vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
#fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
orientation_ecef = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION])
orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
acc_calib = self.calib_from_device.dot(
predicted_state[States.ACCELERATION])
acc_calib_std = np.sqrt(
np.diagonal(
self.calib_from_device.dot(
predicted_cov[States.ACCELERATION_ERR,
States.ACCELERATION_ERR]).dot(
self.calib_from_device.T)))
ang_vel_calib = self.calib_from_device.dot(
predicted_state[States.ANGULAR_VELOCITY])
ang_vel_calib_std = np.sqrt(
np.diagonal(
self.calib_from_device.dot(
predicted_cov[States.ANGULAR_VELOCITY_ERR,
States.ANGULAR_VELOCITY_ERR]).dot(
self.calib_from_device.T)))
device_from_ecef = rot_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
vel_device = device_from_ecef.dot(vel_ecef)
device_from_ecef_eul = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
idxs = list(
range(States.ECEF_ORIENTATION_ERR.start,
States.ECEF_ORIENTATION_ERR.stop)) + list(
range(States.ECEF_VELOCITY_ERR.start,
States.ECEF_VELOCITY_ERR.stop))
condensed_cov = predicted_cov[idxs][:, idxs]
H = self.H(*list(np.concatenate([device_from_ecef_eul, vel_ecef])))
vel_device_cov = H.dot(condensed_cov).dot(H.T)
vel_device_std = np.sqrt(np.diagonal(vel_device_cov))
vel_calib = self.calib_from_device.dot(vel_device)
vel_calib_std = np.sqrt(
np.diagonal(
self.calib_from_device.dot(vel_device_cov).dot(
self.calib_from_device.T)))
orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
#orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
ned_vel = self.converter.ecef2ned(
fix_ecef + vel_ecef) - self.converter.ecef2ned(fix_ecef)
#ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
fix = messaging.log.LiveLocationKalman.new_message()
fix.positionGeodetic.value = to_float(fix_pos_geo)
#fix.positionGeodetic.std = to_float(fix_pos_geo_std)
#fix.positionGeodetic.valid = True
fix.positionECEF.value = to_float(fix_ecef)
fix.positionECEF.std = to_float(fix_ecef_std)
fix.positionECEF.valid = True
fix.velocityECEF.value = to_float(vel_ecef)
fix.velocityECEF.std = to_float(vel_ecef_std)
fix.velocityECEF.valid = True
fix.velocityNED.value = to_float(ned_vel)
#fix.velocityNED.std = to_float(ned_vel_std)
#fix.velocityNED.valid = True
fix.velocityDevice.value = to_float(vel_device)
fix.velocityDevice.std = to_float(vel_device_std)
fix.velocityDevice.valid = True
fix.accelerationDevice.value = to_float(
predicted_state[States.ACCELERATION])
fix.accelerationDevice.std = to_float(
predicted_std[States.ACCELERATION_ERR])
fix.accelerationDevice.valid = True
fix.orientationECEF.value = to_float(orientation_ecef)
fix.orientationECEF.std = to_float(orientation_ecef_std)
fix.orientationECEF.valid = True
fix.orientationNED.value = to_float(orientation_ned)
#fix.orientationNED.std = to_float(orientation_ned_std)
#fix.orientationNED.valid = True
fix.angularVelocityDevice.value = to_float(
predicted_state[States.ANGULAR_VELOCITY])
fix.angularVelocityDevice.std = to_float(
predicted_std[States.ANGULAR_VELOCITY_ERR])
fix.angularVelocityDevice.valid = True
fix.velocityCalibrated.value = to_float(vel_calib)
fix.velocityCalibrated.std = to_float(vel_calib_std)
fix.velocityCalibrated.valid = True
fix.angularVelocityCalibrated.value = to_float(ang_vel_calib)
fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std)
fix.angularVelocityCalibrated.valid = True
fix.accelerationCalibrated.value = to_float(acc_calib)
fix.accelerationCalibrated.std = to_float(acc_calib_std)
fix.accelerationCalibrated.valid = True
#fix.gpsWeek = self.time.week
#fix.gpsTimeOfWeek = self.time.tow
fix.unixTimestampMillis = self.unix_timestamp_millis
if self.filter_ready and self.calibrated:
fix.status = 'valid'
elif self.filter_ready:
fix.status = 'uncalibrated'
else:
fix.status = 'uninitialized'
return fix
def liveLocationMsg(self, time):
predicted_state = self.kf.x
predicted_std = np.diagonal(self.kf.P)
fix_ecef = predicted_state[States.ECEF_POS]
fix_ecef_std = predicted_std[States.ECEF_POS_ERR]
vel_ecef = predicted_state[States.ECEF_VELOCITY]
vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
fix_pos_geo_std = coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo
ned_vel = self.converter.ecef2ned(fix_ecef + vel_ecef) - self.converter.ecef2ned(fix_ecef)
ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
vel_device = device_from_ecef.dot(vel_ecef)
vel_device_std = device_from_ecef.dot(vel_ecef_std)
orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION])
orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
vel_calib = self.calib_from_device.dot(vel_device)
vel_calib_std = self.calib_from_device.dot(vel_device_std)
acc_calib = self.calib_from_device.dot(predicted_state[States.ACCELERATION])
acc_calib_std = self.calib_from_device.dot(predicted_std[States.ACCELERATION_ERR])
ang_vel_calib = self.calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY])
ang_vel_calib_std = self.calib_from_device.dot(predicted_std[States.ANGULAR_VELOCITY_ERR])
fix = messaging.log.LiveLocationKalman.new_message()
fix.positionGeodetic.value = to_float(fix_pos_geo)
fix.positionGeodetic.std = to_float(fix_pos_geo_std)
fix.positionGeodetic.valid = True
fix.positionECEF.value = to_float(fix_ecef)
fix.positionECEF.std = to_float(fix_ecef_std)
fix.positionECEF.valid = True
fix.velocityECEF.value = to_float(vel_ecef)
fix.velocityECEF.std = to_float(vel_ecef_std)
fix.velocityECEF.valid = True
fix.velocityNED.value = to_float(ned_vel)
fix.velocityNED.std = to_float(ned_vel_std)
fix.velocityNED.valid = True
fix.velocityDevice.value = to_float(vel_device)
fix.velocityDevice.std = to_float(vel_device_std)
fix.velocityDevice.valid = True
fix.accelerationDevice.value = to_float(predicted_state[States.ACCELERATION])
fix.accelerationDevice.std = to_float(predicted_std[States.ACCELERATION_ERR])
fix.accelerationDevice.valid = True
fix.orientationECEF.value = to_float(orientation_ecef)
fix.orientationECEF.std = to_float(orientation_ecef_std)
fix.orientationECEF.valid = True
fix.orientationNED.value = to_float(orientation_ned)
fix.orientationNED.std = to_float(orientation_ned_std)
fix.orientationNED.valid = True
fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY])
fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR])
fix.angularVelocityDevice.valid = True
fix.velocityCalibrated.value = to_float(vel_calib)
fix.velocityCalibrated.std = to_float(vel_calib_std)
fix.velocityCalibrated.valid = True
fix.angularVelocityCalibrated.value = to_float(ang_vel_calib)
fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std)
fix.angularVelocityCalibrated.valid = True
fix.accelerationCalibrated.value = to_float(acc_calib)
fix.accelerationCalibrated.std = to_float(acc_calib_std)
fix.accelerationCalibrated.valid = True
#fix.gpsWeek = self.time.week
#fix.gpsTimeOfWeek = self.time.tow
fix.unixTimestampMillis = self.unix_timestamp_millis
if self.filter_ready and self.calibrated:
fix.status = 'valid'
elif self.filter_ready:
fix.status = 'uncalibrated'
else:
fix.status = 'uninitialized'
return fix
