mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-25 16:02:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

need to do something about the valid (#1713)

Browse Source
This commit is contained in:
HaraldSchafer
2020-06-15 17:20:31 -07:00
committed by GitHub
parent be9034b263
commit 2afe224813
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.py
+6 -2
View File
@@ -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