sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Locationd cleanup (#1517)

Browse Source 
* way cleaner take 2

* cleanup

* be more relaxed

* just let it be

* don't drive backwards or upside down

* do this more

* vNED sometyimes invalid

* use reasonble stds

* stability in nonlinear zone

* previous metrics were without publishing
pull/1533/head
HaraldSchafer 5 years ago committed by GitHub
parent 6c46630293
commit 81686547cc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 81 additions and 78 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      common/manager_helpers.py
  2. 111
      selfdrive/locationd/locationd.py
  3. 3
      selfdrive/locationd/models/constants.py
  4. 43
      selfdrive/locationd/models/live_kf.py

2
common/manager_helpers.py
Unescape View File

@ -12,7 +12,7 @@ def print_cpu_usage(first_proc, last_proc):
("selfdrive.controls.radard", 9.54), ("selfdrive.controls.radard", 9.54),
("./_ui", 9.54), ("./_ui", 9.54),
("./camerad", 7.07), ("./camerad", 7.07),
("selfdrive.locationd.locationd", 7.13), ("selfdrive.locationd.locationd", 13.96),
("./_sensord", 6.17), ("./_sensord", 6.17),
("selfdrive.controls.dmonitoringd", 5.48), ("selfdrive.controls.dmonitoringd", 5.48),
("./boardd", 3.63), ("./boardd", 3.63),

111
selfdrive/locationd/locationd.py
Unescape View File

@ -1,16 +1,14 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import math
import numpy as np import numpy as np
import sympy as sp import sympy as sp
import cereal.messaging as messaging import cereal.messaging as messaging
import common.transformations.coordinates as coord import common.transformations.coordinates as coord
from common.transformations.orientation import (ecef_euler_from_ned, from common.transformations.orientation import ecef_euler_from_ned, \
euler_from_quat, euler_from_quat, \
ned_euler_from_ecef, ned_euler_from_ecef, \
quat_from_euler, quat_from_euler, \
rot_from_quat, rot_from_euler) rot_from_quat, rot_from_euler
from rednose.helpers import KalmanError from rednose.helpers import KalmanError
from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind
from selfdrive.locationd.models.constants import GENERATED_DIR from selfdrive.locationd.models.constants import GENERATED_DIR
@ -147,21 +145,20 @@ class Localizer():
#fix.gpsTimeOfWeek = self.time.tow #fix.gpsTimeOfWeek = self.time.tow
fix.unixTimestampMillis = self.unix_timestamp_millis fix.unixTimestampMillis = self.unix_timestamp_millis
if self.filter_ready and self.calibrated: if np.linalg.norm(fix.positionECEF.std) < 50 and self.calibrated:
fix.status = 'valid' fix.status = 'valid'
elif self.filter_ready: elif np.linalg.norm(fix.positionECEF.std) < 50:
fix.status = 'uncalibrated' fix.status = 'uncalibrated'
else: else:
fix.status = 'uninitialized' fix.status = 'uninitialized'
return fix return fix
def update_kalman(self, time, kind, meas): def update_kalman(self, time, kind, meas, R=None):
if self.filter_ready: try:
try: self.kf.predict_and_observe(time, kind, meas, R=R)
self.kf.predict_and_observe(time, kind, meas) except KalmanError:
except KalmanError: cloudlog.error("Error in predict and observe, kalman reset")
cloudlog.error("Error in predict and observe, kalman reset") self.reset_kalman()
self.reset_kalman()
#idx = bisect_right([x[0] for x in self.observation_buffer], time) #idx = bisect_right([x[0] for x in self.observation_buffer], time)
#self.observation_buffer.insert(idx, (time, kind, meas)) #self.observation_buffer.insert(idx, (time, kind, meas))
#while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age: #while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
@ -169,42 +166,38 @@ class Localizer():
# self.observation_buffer.pop(0) # self.observation_buffer.pop(0)
def handle_gps(self, current_time, log): def handle_gps(self, current_time, log):
# ignore the message if the fix is invalid
if log.flags % 2 == 0:
return
self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude]) self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
fix_ecef = self.converter.ned2ecef([0, 0, 0]) ecef_pos = self.converter.ned2ecef([0, 0, 0])
ecef_vel = self.converter.ned2ecef_matrix.dot(np.array(log.vNED))
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.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3))
self.unix_timestamp_millis = log.timestamp 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)
# 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_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_car_state(self, current_time, log): def handle_car_state(self, current_time, log):
self.speed_counter += 1 self.speed_counter += 1
@ -222,12 +215,12 @@ class Localizer():
rot_device_std = self.device_from_calib.dot(log.rotStd) rot_device_std = self.device_from_calib.dot(log.rotStd)
self.update_kalman(current_time, self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_ROTATION,
np.concatenate([rot_device, rot_device_std])) np.concatenate([rot_device, 10*rot_device_std]))
trans_device = self.device_from_calib.dot(log.trans) trans_device = self.device_from_calib.dot(log.trans)
trans_device_std = self.device_from_calib.dot(log.transStd) trans_device_std = self.device_from_calib.dot(log.transStd)
self.update_kalman(current_time, self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_TRANSLATION, ObservationKind.CAMERA_ODO_TRANSLATION,
np.concatenate([trans_device, trans_device_std])) np.concatenate([trans_device, 10*trans_device_std]))
def handle_sensors(self, current_time, log): def handle_sensors(self, current_time, log):
# TODO does not yet account for double sensor readings in the log # TODO does not yet account for double sensor readings in the log
@ -236,10 +229,6 @@ class Localizer():
if sensor_reading.sensor == 5 and sensor_reading.type == 16: if sensor_reading.sensor == 5 and sensor_reading.type == 16:
self.gyro_counter += 1 self.gyro_counter += 1
if self.gyro_counter % SENSOR_DECIMATION == 0: if self.gyro_counter % SENSOR_DECIMATION == 0:
if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07:
cloudlog.info('imu frame angles exceeded, correcting')
self.update_kalman(current_time, ObservationKind.IMU_FRAME, [0, 0, 0])
v = sensor_reading.gyroUncalibrated.v v = sensor_reading.gyroUncalibrated.v
self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]]) self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]])
@ -256,9 +245,14 @@ class Localizer():
self.calib_from_device = self.device_from_calib.T self.calib_from_device = self.device_from_calib.T
self.calibrated = log.calStatus == 1 self.calibrated = log.calStatus == 1
def reset_kalman(self): def reset_kalman(self, current_time=None, init_orient=None):
self.filter_time = None self.filter_time = current_time
self.filter_ready = False init_x = LiveKalman.initial_x
# too nonlinear to init on completely wrong
if init_orient is not None:
init_x[3:7] = init_orient
self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time)
self.observation_buffer = [] self.observation_buffer = []
self.gyro_counter = 0 self.gyro_counter = 0
@ -269,7 +263,8 @@ class Localizer():
def locationd_thread(sm, pm, disabled_logs=[]): def locationd_thread(sm, pm, disabled_logs=[]):
if sm is None: if sm is None:
sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration']) socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration']
sm = messaging.SubMaster(socks)
if pm is None: if pm is None:
pm = messaging.PubMaster(['liveLocationKalman']) pm = messaging.PubMaster(['liveLocationKalman'])
@ -292,7 +287,7 @@ def locationd_thread(sm, pm, disabled_logs=[]):
elif sock == "liveCalibration": elif sock == "liveCalibration":
localizer.handle_live_calib(t, sm[sock]) localizer.handle_live_calib(t, sm[sock])
if localizer.filter_ready and sm.updated['gpsLocationExternal']: if sm.updated['gpsLocationExternal']:
t = sm.logMonoTime['gpsLocationExternal'] t = sm.logMonoTime['gpsLocationExternal']
msg = messaging.new_message('liveLocationKalman') msg = messaging.new_message('liveLocationKalman')
msg.logMonoTime = t msg.logMonoTime = t

3
selfdrive/locationd/models/constants.py
Unescape View File

@ -27,6 +27,8 @@ class ObservationKind:
PSEUDORANGE_RATE_GLONASS = 21 PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22 PSEUDORANGE = 22
PSEUDORANGE_RATE = 23 PSEUDORANGE_RATE = 23
ECEF_VEL = 31
ECEF_ORIENTATION_FROM_GPS = 32
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s] ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s] ROAD_FRAME_YAW_RATE = 25 # [rad/s]
@ -36,6 +38,7 @@ class ObservationKind:
STEER_RATIO = 29 # [-] STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s] ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
names = [ names = [
'Unknown', 'Unknown',
'No observation', 'No observation',

43
selfdrive/locationd/models/live_kf.py
Unescape View File

@ -1,11 +1,11 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import sys import sys
import numpy as np import numpy as np
import sympy as sp import sympy as sp
from selfdrive.locationd.models.constants import ObservationKind from selfdrive.locationd.models.constants import ObservationKind
from rednose.helpers import KalmanError
from rednose.helpers.ekf_sym import EKF_sym, gen_code from rednose.helpers.ekf_sym import EKF_sym, gen_code
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
@ -46,9 +46,9 @@ class LiveKalman():
0, 0, 0]) 0, 0, 0])
# state covariance # state covariance
initial_P_diag = np.array([10000**2, 10000**2, 10000**2, initial_P_diag = np.array([1e14, 1e14, 1e14,
10**2, 10**2, 10**2, 1e6, 1e6, 1e6,
10**2, 10**2, 10**2, 1e4, 1e4, 1e4,
1**2, 1**2, 1**2, 1**2, 1**2, 1**2,
0.05**2, 0.05**2, 0.05**2, 0.05**2, 0.05**2, 0.05**2,
0.02**2, 0.02**2,
@ -57,8 +57,8 @@ class LiveKalman():
# process noise # process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2, Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2, 0.001**2, 0.001*2, 0.001**2,
0.0**2, 0.0**2, 0.0**2, 0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2, 0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02 / 100)**2, (0.02 / 100)**2,
@ -172,6 +172,8 @@ class LiveKalman():
h_speed_sym = sp.Matrix([speed * odo_scale]) h_speed_sym = sp.Matrix([speed * odo_scale])
h_pos_sym = sp.Matrix([x, y, z]) h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
h_imu_frame_sym = sp.Matrix(imu_angles) h_imu_frame_sym = sp.Matrix(imu_angles)
h_relative_motion = sp.Matrix(quat_rot.T * v) h_relative_motion = sp.Matrix(quat_rot.T * v)
@ -181,6 +183,8 @@ class LiveKalman():
[h_phone_rot_sym, ObservationKind.NO_ROT, None], [h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None], [h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None], [h_pos_sym, ObservationKind.ECEF_POS, None],
[h_vel_sym, ObservationKind.ECEF_VEL, None],
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None]] [h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
@ -197,7 +201,9 @@ class LiveKalman():
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]), ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]), ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]), ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])} ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.diag([.5**2, .5**2, .5**2]),
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])}
# init filter # init filter
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err) self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
@ -226,25 +232,24 @@ class LiveKalman():
P = self.filter.covs() P = self.filter.covs()
self.filter.init_state(state, P, filter_time) self.filter.init_state(state, P, filter_time)
def predict_and_observe(self, t, kind, data): def predict_and_observe(self, t, kind, meas, R=None):
if len(data) > 0: if len(meas) > 0:
data = np.atleast_2d(data) meas = np.atleast_2d(meas)
if kind == ObservationKind.CAMERA_ODO_TRANSLATION: if kind == ObservationKind.CAMERA_ODO_TRANSLATION:
r = self.predict_and_update_odo_trans(data, t, kind) r = self.predict_and_update_odo_trans(meas, t, kind)
elif kind == ObservationKind.CAMERA_ODO_ROTATION: elif kind == ObservationKind.CAMERA_ODO_ROTATION:
r = self.predict_and_update_odo_rot(data, t, kind) r = self.predict_and_update_odo_rot(meas, t, kind)
elif kind == ObservationKind.ODOMETRIC_SPEED: elif kind == ObservationKind.ODOMETRIC_SPEED:
r = self.predict_and_update_odo_speed(data, t, kind) r = self.predict_and_update_odo_speed(meas, t, kind)
else: else:
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) if R is None:
R = self.get_R(kind, len(meas))
elif len(R.shape) == 2:
R = R[None]
r = self.filter.predict_and_update_batch(t, kind, meas, R)
# Normalize quats # Normalize quats
quat_norm = np.linalg.norm(self.filter.x[3:7, 0]) quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
# Should not continue if the quats behave this weirdly
if not (0.1 < quat_norm < 10):
raise KalmanError("Kalman filter quaternions unstable")
self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
return r return r

Write Preview
Loading…
Cancel
Save