WIP: Live localizer (#1074)
* cleanup * Proper exception handling * Also check sensor numberalbatross
Willem Melching
GitHub
parent
5388878dac
commit
31794a3d10
|
|
@ -4,6 +4,10 @@ from bisect import bisect
|
|||
from tqdm import tqdm
|
||||
|
||||
|
||||
class KalmanError(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class ObservationKind():
|
||||
UNKNOWN = 0
|
||||
NO_OBSERVATION = 1
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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__)
|
||||
|
|
|
|||
|
|
@ -1,16 +1,23 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import zmq
|
||||
import numpy as np
|
||||
import math
|
||||
from bisect import bisect_right
|
||||
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 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"
|
||||
import numpy as np
|
||||
|
||||
import cereal.messaging as messaging
|
||||
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
|
||||
|
||||
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()
|
||||
|
||||
#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)
|
||||
|
||||
liveLocation = messaging.pub_sock('liveLocation')
|
||||
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'])
|
||||
|
||||
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)
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
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()
|
||||
|
|
|
|||
Loading…
Reference in New Issue