spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

more fixes

pull/1076/head
Harald Schafer 2020-02-10 19:53:43 -08:00
parent 31794a3d10
commit 1a1f1182d7
3 changed files with 35 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
selfdrive/locationd/kalman/live_kf.py
View File

@ -2,7 +2,7 @@
import numpy as np
from selfdrive.swaglog import cloudlog
from selfdrive.locationd.kalman.live_model import gen_model
from selfdrive.locationd.kalman.live_model import gen_model, States
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind, KalmanError
from selfdrive.locationd.kalman.ekf_sym import EKF_sym
@ -29,7 +29,7 @@ initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
class LiveKalman():
def __init__(self, N=0, max_tracks=3000):
def __init__(self):
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2,
@ -52,7 +52,7 @@ 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 = f'live_{N}'
name = 'live'
gen_model(name, self.dim_state, self.dim_state_err, [])
# init filter
@ -105,7 +105,7 @@ class LiveKalman():
cloudlog.error("Kalman filter quaternions unstable")
raise KalmanError
self.filter.x[3:7, 0] = self.filter.x[3:7, 0] / quat_norm
self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
return r

9
selfdrive/locationd/kalman/live_model.py
View File

@ -1,6 +1,7 @@
import numpy as np
import sympy as sp
import os
import sysconfig
from laika.constants import EARTH_GM
from common.sympy_helpers import euler_rotate, quat_rotate, quat_matrix_r
@ -38,7 +39,7 @@ def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
dir_path + '/' + name + '_kf.py']
outs = [dir_path + '/' + name + '.o',
dir_path + '/' + name + '.so',
dir_path + '/' + name + sysconfig.get_config_var('EXT_SUFFIX'),
dir_path + '/' + name + '.cpp']
out_times = list(map(os.path.getmtime, outs))
dep_times = list(map(os.path.getmtime, deps))
@ -46,7 +47,9 @@ def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
if min(out_times) > max(dep_times) and not rebuild:
return
list(map(os.remove, outs))
except OSError:
except OSError as e:
print('HAHAHA')
print(e)
pass
# make functions and jacobians with sympy
@ -122,7 +125,7 @@ def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
err_function_sym = sp.Matrix(np.zeros((dim_state,1)))
delta_quat = sp.Matrix(np.ones((4)))
delta_quat[1:,:] = sp.Matrix(0.5*delta_x[3:6,:])
err_function_sym[3:7,0] = quat_matrix_r(nom_x[3:6,0])*delta_quat
err_function_sym[3:7,0] = quat_matrix_r(nom_x[3:7,0])*delta_quat
err_function_sym[0:3,:] = sp.Matrix(nom_x[0:3,:] + delta_x[0:3,:])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err,1)))

59
selfdrive/locationd/locationd.py
View File

@ -49,8 +49,8 @@ class Localizer():
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 = math.degrees(math.arctan2(local_vel[2], local_vel[0]))
fix.yawCalibration = math.degrees(math.arctan2(local_vel[1], local_vel[0]))
fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0]))
fix.yawCalibration = math.degrees(math.atan2(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
@ -58,7 +58,7 @@ class Localizer():
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:
while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
if self.filter_ready:
try:
self.kf.predict_and_observe(*self.observation_buffer.pop(0))
@ -68,18 +68,18 @@ class Localizer():
else:
self.observation_buffer.pop(0)
def handle_gps(self, log, current_time):
converter = coord.LocalCoord.from_geodetic([log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude])
def handle_gps(self, current_time, log):
converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.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:
if not self.filter_ready and log.speed > 5:
self.filter_ready = True
initial_ecef = fix_ecef
gps_bearing = math.radians(log.gpsLocationExternal.bearing)
gps_bearing = math.radians(log.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
gps_speed = log.speed
quat_uncertainty = 0.2**2
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
@ -105,25 +105,25 @@ class Localizer():
cloudlog.error("Locationd vs ubloxLocation difference too large, kalman reset")
self.reset_kalman()
def handle_car_state(self, log, current_time):
def handle_car_state(self, current_time, log):
self.speed_counter += 1
if self.speed_counter % SENSOR_DECIMATION == 0:
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, log.carState.vEgo)
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, log.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):
def handle_cam_odo(self, current_time, log):
self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_ROTATION,
np.concatenate([log.cameraOdometry.rot, log.cameraOdometry.rotStd]))
np.concatenate([log.rot, log.rotStd]))
self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_TRANSLATION,
np.concatenate([log.cameraOdometry.trans, log.cameraOdometry.transStd]))
np.concatenate([log.trans, log.transStd]))
def handle_sensors(self, log, current_time):
def handle_sensors(self, current_time, log):
# TODO does not yet account for double sensor readings in the log
for sensor_reading in log.sensorEvents:
for sensor_reading in log:
# Gyro Uncalibrated
if sensor_reading.sensor == 5 and sensor_reading.type == 16:
self.gyro_counter += 1
@ -142,23 +142,6 @@ class Localizer():
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
typ = log.which
if typ in self.disabled_logs:
return
if typ == "sensorEvents":
self.handle_sensors(log, current_time)
elif typ == "gpsLocationExternal":
self.handle_gps(log, current_time)
elif typ == "carState":
self.handle_car_state(log, current_time)
elif typ == "cameraOdometry":
self.handle_cam_odo(log, current_time)
def reset_kalman(self):
self.filter_time = None
self.filter_ready = False
@ -171,7 +154,7 @@ class Localizer():
def locationd_thread(sm, pm, disabled_logs=[]):
if sm is None:
sm = messaging.SubMaster(['carState', 'gpsLocationExternal', 'sensorEvents', 'cameraOdometry'])
sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry'])
if pm is None:
pm = messaging.PubMaster(['liveLocation'])
@ -182,7 +165,15 @@ def locationd_thread(sm, pm, disabled_logs=[]):
for sock, updated in sm.updated.items():
if updated:
localizer.handle_log(sm[sock])
t = sm.logMonoTime[sock] * 1e-9
if sock == "sensorEvents":
localizer.handle_sensors(t, sm[sock])
elif sock == "gpsLocationExternal":
localizer.handle_gps(t, sm[sock])
elif sock == "carState":
localizer.handle_car_state(t, sm[sock])
elif sock == "cameraOdometry":
localizer.handle_cam_odo(t, sm[sock])
if localizer.filter_ready and sm.updated['gpsLocationExternal']:
t = sm.logMonoTime['gpsLocationExternal']