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()
def handle_gps(self, log, current_time): self.converter = coord.LocalCoord.from_geodetic([ log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude ]) fix_ecef = self.converter.ned2ecef([0, 0, 0]) # initing with bad bearing allowed, maybe bad? if not self.filter_ready and len(list(self.dog.orbits.keys( ))) > 6: # and log.gpsLocationExternal.speed > 5: self.filter_ready = True initial_ecef = fix_ecef initial_state = np.zeros(29) gps_bearing = log.gpsLocationExternal.bearing * (np.pi / 180) initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing]) initial_pose_ecef_quat = euler2quat(initial_pose_ecef) gps_speed = log.gpsLocationExternal.speed quat_uncertainty = 0.2**2 initial_pose_ecef_quat = euler2quat(initial_pose_ecef) initial_state[:3] = initial_ecef initial_state[3:7] = initial_pose_ecef_quat initial_state[7:10] = rotations_from_quats( initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) initial_state[18] = 1 initial_state[22] = 1 covs_diag = np.array([ 10**2, 10**2, 10**2, quat_uncertainty, quat_uncertainty, quat_uncertainty, 2**2, 2**2, 2**2, 1, 1, 1, 20000000**2, 100**2, 0.01**2, 0.01**2, 0.01**2, 0.02**2, 2**2, 2**2, 2**2, .01**2, 0.01**2, 0.01**2, 0.01**2, 10**2, 1**2, 0.2**2 ]) self.kf.init_state(initial_state, covs=np.diag(covs_diag), filter_time=current_time) print("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.info( "Locationd vs ubloxLocation difference too large, kalman reset" ) self.reset_kalman()