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
old-commit-hash: 81686547cc
commatwo_master
HaraldSchafer 5 years ago committed by GitHub
parent 96dd0232cd
commit e21bbf8bbd
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
Split 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),
("./_ui", 9.54),
("./camerad", 7.07),
("selfdrive.locationd.locationd", 7.13),
("selfdrive.locationd.locationd", 13.96),
("./_sensord", 6.17),
("selfdrive.controls.dmonitoringd", 5.48),
("./boardd", 3.63),

111
selfdrive/locationd/locationd.py
Unescape View File

@ -1,16 +1,14 @@
#!/usr/bin/env python3
import math
import numpy as np
import sympy as sp
import cereal.messaging as messaging
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,
rot_from_quat, rot_from_euler)
from common.transformations.orientation import ecef_euler_from_ned, \
euler_from_quat, \
ned_euler_from_ecef, \
quat_from_euler, \
rot_from_quat, rot_from_euler
from rednose.helpers import KalmanError
from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind
from selfdrive.locationd.models.constants import GENERATED_DIR
@ -147,21 +145,20 @@ class Localizer():
#fix.gpsTimeOfWeek = self.time.tow
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'
elif self.filter_ready:
elif np.linalg.norm(fix.positionECEF.std) < 50:
fix.status = 'uncalibrated'
else:
fix.status = 'uninitialized'
return fix
def update_kalman(self, time, kind, meas):
if self.filter_ready:
try:
self.kf.predict_and_observe(time, kind, meas)
except KalmanError:
cloudlog.error("Error in predict and observe, kalman reset")
self.reset_kalman()
def update_kalman(self, time, kind, meas, R=None):
try:
self.kf.predict_and_observe(time, kind, meas, R=R)
except KalmanError:
cloudlog.error("Error in predict and observe, kalman reset")
self.reset_kalman()
#idx = bisect_right([x[0] for x in self.observation_buffer], time)
#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:
@ -169,42 +166,38 @@ class Localizer():
# self.observation_buffer.pop(0)
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])
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.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):
self.speed_counter += 1
@ -222,12 +215,12 @@ class Localizer():
rot_device_std = self.device_from_calib.dot(log.rotStd)
self.update_kalman(current_time,
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_std = self.device_from_calib.dot(log.transStd)
self.update_kalman(current_time,
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):
# 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:
self.gyro_counter += 1
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
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.calibrated = log.calStatus == 1
def reset_kalman(self):
self.filter_time = None
self.filter_ready = False
def reset_kalman(self, current_time=None, init_orient=None):
self.filter_time = current_time
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.gyro_counter = 0
@ -269,7 +263,8 @@ class Localizer():
def locationd_thread(sm, pm, disabled_logs=[]):
if sm is None:
sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration'])
socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration']
sm = messaging.SubMaster(socks)
if pm is None:
pm = messaging.PubMaster(['liveLocationKalman'])
@ -292,7 +287,7 @@ def locationd_thread(sm, pm, disabled_logs=[]):
elif sock == "liveCalibration":
localizer.handle_live_calib(t, sm[sock])
if localizer.filter_ready and sm.updated['gpsLocationExternal']:
if sm.updated['gpsLocationExternal']:
t = sm.logMonoTime['gpsLocationExternal']
msg = messaging.new_message('liveLocationKalman')
msg.logMonoTime = t

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

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

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

@ -1,11 +1,11 @@
#!/usr/bin/env python3
import sys
import numpy as np
import sympy as sp
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.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
@ -46,9 +46,9 @@ class LiveKalman():
0, 0, 0])
# state covariance
initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
10**2, 10**2, 10**2,
10**2, 10**2, 10**2,
initial_P_diag = np.array([1e14, 1e14, 1e14,
1e6, 1e6, 1e6,
1e4, 1e4, 1e4,
1**2, 1**2, 1**2,
0.05**2, 0.05**2, 0.05**2,
0.02**2,
@ -57,8 +57,8 @@ class LiveKalman():
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2,
0.0**2, 0.0**2, 0.0**2,
0.001**2, 0.001*2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02 / 100)**2,
@ -172,6 +172,8 @@ class LiveKalman():
h_speed_sym = sp.Matrix([speed * odo_scale])
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_relative_motion = sp.Matrix(quat_rot.T * v)
@ -181,6 +183,8 @@ class LiveKalman():
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, 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_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, 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.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.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
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()
self.filter.init_state(state, P, filter_time)
def predict_and_observe(self, t, kind, data):
if len(data) > 0:
data = np.atleast_2d(data)
def predict_and_observe(self, t, kind, meas, R=None):
if len(meas) > 0:
meas = np.atleast_2d(meas)
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:
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:
r = self.predict_and_update_odo_speed(data, t, kind)
r = self.predict_and_update_odo_speed(meas, t, kind)
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
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
return r

Write Preview
Loading…
Cancel
Save