diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index ae2c76165256ad4..ebe7404e17618e4 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -93,15 +93,12 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist() vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist() - bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std) - meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid), "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid), - "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=kf_valid), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } @@ -210,16 +207,6 @@ def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMe gnss_kf.predict_and_observe(t, kind, data) -def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std): - # init orientation with direction of velocity - converter = coord.LocalCoord.from_ecef(ecef_pos) - - ned_vel = np.einsum('ij,j ->i', converter.ned_from_ecef_matrix, ecef_vel) - bearing = np.arctan2(ned_vel[1], ned_vel[0]) - bearing_std = np.arctan2(np.linalg.norm(vel_std), np.linalg.norm(ned_vel)) - return float(np.rad2deg(bearing)), float(bearing_std) - - class CacheSerializer(json.JSONEncoder): def default(self, o):