Locationd cleanup (#1517)
* 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 publishingalbatross
HaraldSchafer
GitHub
parent
6c46630293
commit
81686547cc
|
|
@ -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),
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
# 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
|
||||
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()
|
||||
|
||||
initial_state = LiveKalman.initial_x
|
||||
initial_covs_diag = LiveKalman.initial_P_diag
|
||||
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)
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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',
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue