From c26f15d608950d50adfd63f84184ca1d682257e4 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Mon, 15 Jun 2020 17:20:31 -0700 Subject: [PATCH] need to do something about the valid (#1713) old-commit-hash: 2afe22481348920d490daad32be1ce9898a7ab89 --- selfdrive/locationd/locationd.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 00f31b644c..155174aa60 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -7,7 +7,7 @@ import common.transformations.coordinates as coord from common.transformations.orientation import ecef_euler_from_ned, \ euler_from_quat, \ ned_euler_from_ecef, \ - quat_from_euler, \ + quat_from_euler, euler_from_rot, \ rot_from_quat, rot_from_euler from rednose.helpers import KalmanError from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind @@ -81,6 +81,8 @@ class Localizer(): #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( @@ -91,7 +93,6 @@ class Localizer(): 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)) + \ @@ -133,6 +134,9 @@ class Localizer(): fix.orientationECEF.value = to_float(orientation_ecef) fix.orientationECEF.std = to_float(orientation_ecef_std) fix.orientationECEF.valid = True + fix.calibratedOrientationECEF.value = to_float(calibrated_orientation_ecef) + #fix.calibratedOrientationECEF.std = to_float(calibrated_orientation_ecef_std) + #fix.calibratedOrientationECEF.valid = True fix.orientationNED.value = to_float(orientation_ned) #fix.orientationNED.std = to_float(orientation_ned_std) #fix.orientationNED.valid = True