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