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

WIP: Live localizer (#1074)

Browse Source 
* cleanup

* Proper exception handling

* Also check sensor number
pull/1076/head
Willem Melching 5 years ago committed by GitHub
parent 5388878dac
commit 31794a3d10
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 117 additions and 87 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. 4
      selfdrive/locationd/kalman/kalman_helpers.py
  2. 42
      selfdrive/locationd/kalman/live_kf.py
  3. 10
      selfdrive/locationd/kalman/live_model.py
  4. 148
      selfdrive/locationd/locationd.py

4
selfdrive/locationd/kalman/kalman_helpers.py
Unescape View File

@ -4,6 +4,10 @@ from bisect import bisect
from tqdm import tqdm
class KalmanError(Exception):
pass
class ObservationKind():
UNKNOWN = 0
NO_OBSERVATION = 1

42
selfdrive/locationd/kalman/live_kf.py
Unescape View File

@ -1,9 +1,10 @@
#!/usr/bin/env python3
import numpy as np
from .live_model import gen_model, States
from .kalman_helpers import ObservationKind
from .ekf_sym import EKF_sym
from selfdrive.swaglog import cloudlog
from selfdrive.locationd.kalman.live_model import gen_model
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind, KalmanError
from selfdrive.locationd.kalman.ekf_sym import EKF_sym
initial_x = np.array([-2.7e6, 4.2e6, 3.8e6,
@ -29,8 +30,6 @@ initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
class LiveKalman():
def __init__(self, N=0, max_tracks=3000):
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2,
@ -42,6 +41,9 @@ class LiveKalman():
0.001**2,
(0.05/60)**2, (0.05/60)**2, (0.05/60)**2])
self.dim_state = initial_x.shape[0]
self.dim_state_err = initial_P_diag.shape[0]
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5*2]),
@ -50,9 +52,8 @@ class LiveKalman():
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])}
name = 'live' % N
gen_model(name, self.dim_state, self.dim_state_err)
name = f'live_{N}'
gen_model(name, self.dim_state, self.dim_state_err, [])
# init filter
self.filter = EKF_sym(name, Q, initial_x, np.diag(initial_P_diag), self.dim_state, self.dim_state_err)
@ -95,12 +96,17 @@ class LiveKalman():
r = self.predict_and_update_odo_speed(data, t, kind)
else:
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
# 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 RuntimeError("Sir! The filter's gone all wobbly!")
self.filter.x[3:7,0] = self.filter.x[3:7,0]/quat_norm
if not (0.1 < quat_norm < 10):
cloudlog.error("Kalman filter quaternions unstable")
raise KalmanError
self.filter.x[3:7, 0] = self.filter.x[3:7, 0] / quat_norm
return r
def get_R(self, kind, n):
@ -108,28 +114,28 @@ class LiveKalman():
dim = obs_noise.shape[0]
R = np.zeros((n, dim, dim))
for i in range(n):
R[i,:,:] = obs_noise
R[i, :, :] = obs_noise
return R
def predict_and_update_odo_speed(self, speed, t, kind):
z = np.array(speed)
R = np.zeros((len(speed), 1, 1))
for i, _ in enumerate(z):
R[i,:,:] = np.diag([0.2**2])
R[i, :, :] = np.diag([0.2**2])
return self.filter.predict_and_update_batch(t, kind, z, R)
def predict_and_update_odo_trans(self, trans, t, kind):
z = trans[:,:3]
z = trans[:, :3]
R = np.zeros((len(trans), 3, 3))
for i, _ in enumerate(z):
R[i,:,:] = np.diag(trans[i,3:]**2)
R[i, :, :] = np.diag(trans[i, 3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)
def predict_and_update_odo_rot(self, rot, t, kind):
z = rot[:,:3]
z = rot[:, :3]
R = np.zeros((len(rot), 3, 3))
for i, _ in enumerate(z):
R[i,:,:] = np.diag(rot[i,3:]**2)
R[i, :, :] = np.diag(rot[i, 3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)

10
selfdrive/locationd/kalman/live_model.py
Unescape View File

@ -3,9 +3,9 @@ import sympy as sp
import os
from laika.constants import EARTH_GM
from .kalman_helpers import ObservationKind
from .ekf_sym import gen_code
from common.sympy_helpers import euler_rotate, quat_rotate, quat_matrix_r
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
from selfdrive.locationd.kalman.ekf_sym import gen_code
class States():
@ -28,11 +28,7 @@ class States():
IMU_OFFSET_ERR = slice(19, 22)
def gen_model(name,
dim_state, dim_state_err,
maha_test_kinds):
def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
# check if rebuild is needed
try:
dir_path = os.path.dirname(__file__)

148
selfdrive/locationd/locationd.py
Unescape View File

@ -1,16 +1,23 @@
#!/usr/bin/env python3
import os
import zmq
import numpy as np
import math
from bisect import bisect_right
import numpy as np
import cereal.messaging as messaging
from selfdrive.swaglog import cloudlog
from common.transformations.orientation import rotations_from_quats, ecef_euler_from_ned, euler2quat, ned_euler_from_ecef, quat2euler
import common.transformations.coordinates as coord
from common.transformations.orientation import (ecef_euler_from_ned,
euler2quat,
ned_euler_from_ecef,
quat2euler,
rotations_from_quats)
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind, KalmanError
from selfdrive.locationd.kalman.live_kf import (LiveKalman, initial_P_diag,
initial_x)
from selfdrive.locationd.kalman.live_model import States
from selfdrive.swaglog import cloudlog
from selfdrive.locationd.kalman.live_kf import LiveKalman, States, initial_x, initial_P_diag
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
os.environ["OMP_NUM_THREADS"] = "1"
SENSOR_DECIMATION = 1 # No decimation
class Localizer():
@ -19,11 +26,12 @@ class Localizer():
self.reset_kalman()
self.max_age = .2 # seconds
self.disabled_logs = disabled_logs
self.week = None
def liveLocationMsg(self, time):
fix = messaging.log.LiveLocationData.new_message()
predicted_state = self.kf.x
fix_ecef = predicted_state[States.ECEF_POS]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
fix.lat = float(fix_pos_geo[0])
@ -33,36 +41,42 @@ class Localizer():
fix.speed = float(np.linalg.norm(predicted_state[States.ECEF_VELOCITY]))
orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION]))
fix.roll = float(orientation_ned_euler[0]*180/np.pi)
fix.pitch = float(orientation_ned_euler[1]*180/np.pi)
fix.heading = float(orientation_ned_euler[2]*180/np.pi)
fix.roll = math.degrees(orientation_ned_euler[0])
fix.pitch = math.degrees(orientation_ned_euler[1])
fix.heading = math.degrees(orientation_ned_euler[2])
fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])]
fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])]
local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY])
fix.pitchCalibration = float((np.arctan2(local_vel[2], local_vel[0]))*180/np.pi)
fix.yawCalibration = float((np.arctan2(local_vel[1], local_vel[0]))*180/np.pi)
fix.pitchCalibration = math.degrees(math.arctan2(local_vel[2], local_vel[0]))
fix.yawCalibration = math.degrees(math.arctan2(local_vel[1], local_vel[0]))
#fix.imuFrame = [(180/np.pi)*float(predicted_state[23]), (180/np.pi)*float(predicted_state[24]), (180/np.pi)*float(predicted_state[25])]
return fix
def update_kalman(self, time, kind, meas):
idx = bisect_right([x[0] for x in self.observation_buffer], time)
self.observation_buffer.insert(idx, (time, kind, meas))
while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
if self.filter_ready:
self.kf.predict_and_observe(*self.observation_buffer.pop(0))
try:
self.kf.predict_and_observe(*self.observation_buffer.pop(0))
except KalmanError:
cloudlog.error("Error in predict and observe, kalman reset")
self.reset_kalman()
else:
self.observation_buffer.pop(0)
def handle_gps(self, log, current_time):
self.converter = coord.LocalCoord.from_geodetic([log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude])
fix_ecef = self.converter.ned2ecef([0,0,0])
converter = coord.LocalCoord.from_geodetic([log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude])
fix_ecef = converter.ned2ecef([0, 0, 0])
# TODO initing with bad bearing not allowed, maybe not bad?
if not self.filter_ready and log.gpsLocationExternal.speed > 5:
self.filter_ready = True
initial_ecef = fix_ecef
gps_bearing = log.gpsLocationExternal.bearing*(np.pi/180)
gps_bearing = math.radians(log.gpsLocationExternal.bearing)
initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing])
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
gps_speed = log.gpsLocationExternal.speed
@ -80,49 +94,62 @@ class Localizer():
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)
print("Filter initialized")
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.info("Locationd vs ubloxLocation difference too large, kalman reset")
cloudlog.error("Locationd vs ubloxLocation difference too large, kalman reset")
self.reset_kalman()
def handle_car_state(self, log, current_time):
self.speed_counter += 1
if self.speed_counter % 5==0:
if self.speed_counter % SENSOR_DECIMATION == 0:
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, log.carState.vEgo)
if log.carState.vEgo == 0:
self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0])
def handle_cam_odo(self, log, current_time):
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot,
log.cameraOdometry.rotStd]))
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
log.cameraOdometry.transStd]))
self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_ROTATION,
np.concatenate([log.cameraOdometry.rot, log.cameraOdometry.rotStd]))
self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_TRANSLATION,
np.concatenate([log.cameraOdometry.trans, log.cameraOdometry.transStd]))
def handle_sensors(self, log, current_time):
# TODO does not yet account for double sensor readings in the log
for sensor_reading in log.sensorEvents:
# TODO does not yet account for double sensor readings in the log
if sensor_reading.type == 4:
# Gyro Uncalibrated
if sensor_reading.sensor == 5 and sensor_reading.type == 16:
self.gyro_counter += 1
if True or self.gyro_counter % 5==0:
if self.gyro_counter % SENSOR_DECIMATION == 0:
if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07:
print('imu frame angles exceeded, correcting')
cloudlog.info('imu frame angles exceeded, correcting')
self.update_kalman(current_time, ObservationKind.IMU_FRAME, [0, 0, 0])
self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]])
if sensor_reading.type == 1:
v = sensor_reading.gyroUncalibrated.v
self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]])
# Accelerometer
if sensor_reading.sensor == 1 and sensor_reading.type == 1:
self.acc_counter += 1
if True or self.acc_counter % 5==0:
self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-sensor_reading.acceleration.v[2], -sensor_reading.acceleration.v[1], -sensor_reading.acceleration.v[0]])
if self.acc_counter % SENSOR_DECIMATION == 0:
v = sensor_reading.acceleration.v
self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]])
def handle_log(self, log):
current_time = 1e-9*log.logMonoTime
current_time = 1e-9 * log.logMonoTime
typ = log.which
if typ in self.disabled_logs:
return
if typ == "sensorEvents":
self.handle_sensors(log, current_time)
elif typ == "gpsLocationExternal":
@ -136,46 +163,43 @@ class Localizer():
self.filter_time = None
self.filter_ready = False
self.observation_buffer = []
self.converter = None
self.gyro_counter = 0
self.acc_counter = 0
self.speed_counter = 0
def locationd_thread(gctx, addr, disabled_logs=[]):
poller = zmq.Poller()
def locationd_thread(sm, pm, disabled_logs=[]):
if sm is None:
sm = messaging.SubMaster(['carState', 'gpsLocationExternal', 'sensorEvents', 'cameraOdometry'])
if pm is None:
pm = messaging.PubMaster(['liveLocation'])
#carstate = messaging.sub_sock('carState', poller, addr=addr, conflate=True)
gpsLocationExternal = messaging.sub_sock('gpsLocationExternal', poller, addr=addr, conflate=True)
sensorEvents = messaging.sub_sock('sensorEvents', poller, addr=addr, conflate=True)
cameraOdometry = messaging.sub_sock('cameraOdometry', poller, addr=addr, conflate=True)
localizer = Localizer(disabled_logs=disabled_logs)
liveLocation = messaging.pub_sock('liveLocation')
while True:
sm.update()
localizer = Localizer(disabled_logs=disabled_logs)
print("init done")
# buffer with all the messages that still need to be input into the kalman
while 1:
polld = poller.poll(timeout=1000)
for sock, mode in polld:
if mode != zmq.POLLIN:
continue
logs = messaging.drain_sock(sock)
for log in logs:
localizer.handle_log(log)
if localizer.filter_ready and log.which == 'ubloxGnss':
for sock, updated in sm.updated.items():
if updated:
localizer.handle_log(sm[sock])
if localizer.filter_ready and sm.updated['gpsLocationExternal']:
t = sm.logMonoTime['gpsLocationExternal']
msg = messaging.new_message()
msg.logMonoTime = log.logMonoTime
msg.logMonoTime = t
msg.init('liveLocation')
msg.liveLocation = localizer.liveLocationMsg(log.logMonoTime*1e-9)
liveLocation.send(msg.to_bytes())
msg.liveLocation = localizer.liveLocationMsg(t * 1e-9)
pm.send('liveLocation', msg)
def main(gctx=None, addr="127.0.0.1"):
locationd_thread(gctx, addr)
def main(sm=None, pm=None):
locationd_thread(sm, pm)
if __name__ == "__main__":
import os
os.environ["OMP_NUM_THREADS"] = "1"
main()

Write Preview
Loading…
Cancel
Save