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 handle_gps(self, current_time, log):
self.converter = coord.LocalCoord.from_geodetic(
[log.latitude, log.longitude, log.altitude])
fix_ecef = self.converter.ned2ecef([0, 0, 0])
#self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3))
self.unix_timestamp_millis = log.timestamp
# TODO initing with bad bearing not allowed, maybe not bad?
if not self.filter_ready and log.speed > 5:
self.filter_ready = True
initial_ecef = fix_ecef
gps_bearing = math.radians(log.bearing)
initial_pose_ecef = ecef_euler_from_ned(initial_ecef,
[0, 0, gps_bearing])
initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef)
gps_speed = log.speed
quat_uncertainty = 0.2**2
initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef)
initial_state = LiveKalman.initial_x
initial_covs_diag = LiveKalman.initial_P_diag
initial_state[States.ECEF_POS] = initial_ecef
initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat
initial_state[States.ECEF_VELOCITY] = rot_from_quat(
initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))
initial_covs_diag[States.ECEF_POS_ERR] = 10**2
initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty
initial_covs_diag[States.ECEF_VELOCITY_ERR] = 1**2
self.kf.init_state(initial_state,
covs=np.diag(initial_covs_diag),
filter_time=current_time)
cloudlog.info("Filter initialized")
elif self.filter_ready:
self.update_kalman(current_time, ObservationKind.ECEF_POS,
fix_ecef)
gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 +
(self.kf.x[1] - fix_ecef[1])**2 +
(self.kf.x[2] - fix_ecef[2])**2)
if gps_est_error > 50:
cloudlog.error(
"Locationd vs ubloxLocation difference too large, kalman reset"
)
self.reset_kalman()
