Misc locationd improvements (#1714)
* I like this more * rewind less * bump rednose * falling off windshield detectopr * adjust thresholds * this is a soft disable now * move that * process replay fixes * update refs Co-authored-by: Comma Device <device@comma.ai> Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>albatross
parent
4658df7a66
commit
8e2d344135
|
@ -1 +1 @@
|
|||
Subproject commit 307c771fcaa5e301ccb18d3ab9fdedddd675c569
|
||||
Subproject commit 2e556b8219185708ed974a4b6502796607d7ce0d
|
|
@ -530,15 +530,6 @@ EVENTS = {
|
|||
duration_hud_alert=0.),
|
||||
},
|
||||
|
||||
EventName.posenetInvalid: {
|
||||
ET.WARNING: Alert(
|
||||
"TAKE CONTROL",
|
||||
"Vision Model Output Uncertain",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
|
||||
ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"),
|
||||
},
|
||||
|
||||
EventName.focusRecoverActive: {
|
||||
ET.WARNING: Alert(
|
||||
"TAKE CONTROL",
|
||||
|
@ -659,6 +650,11 @@ EVENTS = {
|
|||
ET.NO_ENTRY : NoEntryAlert("Driving model lagging"),
|
||||
},
|
||||
|
||||
EventName.posenetInvalid: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Vision Model Output Uncertain"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"),
|
||||
},
|
||||
|
||||
EventName.lowMemory: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"),
|
||||
ET.PERMANENT: Alert(
|
||||
|
|
|
@ -1,7 +1,6 @@
|
|||
#!/usr/bin/env python3
|
||||
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, \
|
||||
|
@ -23,6 +22,7 @@ from rednose.helpers.sympy_helpers import euler_rotate
|
|||
|
||||
VISION_DECIMATION = 2
|
||||
SENSOR_DECIMATION = 10
|
||||
POSENET_STD_HIST = 40
|
||||
|
||||
|
||||
def to_float(arr):
|
||||
|
@ -52,7 +52,7 @@ class Localizer():
|
|||
|
||||
self.kf = LiveKalman(GENERATED_DIR)
|
||||
self.reset_kalman()
|
||||
self.max_age = .2 # seconds
|
||||
self.max_age = .1 # seconds
|
||||
self.disabled_logs = disabled_logs
|
||||
self.calib = np.zeros(3)
|
||||
self.device_from_calib = np.eye(3)
|
||||
|
@ -63,6 +63,7 @@ class Localizer():
|
|||
self.posenet_invalid_count = 0
|
||||
self.posenet_speed = 0
|
||||
self.car_speed = 0
|
||||
self.posenet_stds = 10*np.ones((POSENET_STD_HIST))
|
||||
|
||||
self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS])
|
||||
|
||||
|
@ -113,8 +114,8 @@ class Localizer():
|
|||
|
||||
fix = messaging.log.LiveLocationKalman.new_message()
|
||||
fix.positionGeodetic.value = to_float(fix_pos_geo)
|
||||
#fix.positionGeodetic.std = to_float(fix_pos_geo_std)
|
||||
#fix.positionGeodetic.valid = True
|
||||
fix.positionGeodetic.std = to_float(np.nan*np.zeros(3))
|
||||
fix.positionGeodetic.valid = True
|
||||
fix.positionECEF.value = to_float(fix_ecef)
|
||||
fix.positionECEF.std = to_float(fix_ecef_std)
|
||||
fix.positionECEF.valid = True
|
||||
|
@ -122,8 +123,8 @@ class Localizer():
|
|||
fix.velocityECEF.std = to_float(vel_ecef_std)
|
||||
fix.velocityECEF.valid = True
|
||||
fix.velocityNED.value = to_float(ned_vel)
|
||||
#fix.velocityNED.std = to_float(ned_vel_std)
|
||||
#fix.velocityNED.valid = True
|
||||
fix.velocityNED.std = to_float(np.nan*np.zeros(3))
|
||||
fix.velocityNED.valid = True
|
||||
fix.velocityDevice.value = to_float(vel_device)
|
||||
fix.velocityDevice.std = to_float(vel_device_std)
|
||||
fix.velocityDevice.valid = True
|
||||
|
@ -135,11 +136,11 @@ class Localizer():
|
|||
fix.orientationECEF.std = to_float(orientation_ecef_std)
|
||||
fix.orientationECEF.valid = True
|
||||
fix.calibratedOrientationECEF.value = to_float(calibrated_orientation_ecef)
|
||||
#fix.calibratedOrientationECEF.std = to_float(calibrated_orientation_ecef_std)
|
||||
#fix.calibratedOrientationECEF.valid = True
|
||||
fix.calibratedOrientationECEF.std = to_float(np.nan*np.zeros(3))
|
||||
fix.calibratedOrientationECEF.valid = True
|
||||
fix.orientationNED.value = to_float(orientation_ned)
|
||||
#fix.orientationNED.std = to_float(orientation_ned_std)
|
||||
#fix.orientationNED.valid = True
|
||||
fix.orientationNED.std = to_float(np.nan*np.zeros(3))
|
||||
fix.orientationNED.valid = True
|
||||
fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY])
|
||||
fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR])
|
||||
fix.angularVelocityDevice.valid = True
|
||||
|
@ -155,14 +156,23 @@ class Localizer():
|
|||
fix.accelerationCalibrated.valid = True
|
||||
return fix
|
||||
|
||||
def liveLocationMsg(self, time):
|
||||
def liveLocationMsg(self):
|
||||
fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P)
|
||||
|
||||
if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0):
|
||||
self.posenet_invalid_count += 1
|
||||
#if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0):
|
||||
# self.posenet_invalid_count += 1
|
||||
#else:
|
||||
# self.posenet_invalid_count = 0
|
||||
#fix.posenetOK = self.posenet_invalid_count < 4
|
||||
|
||||
# experimentally found these values
|
||||
old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:])
|
||||
std_spike = new_mean/old_mean > 4 and new_mean > 5
|
||||
|
||||
if std_spike and self.car_speed > 5:
|
||||
fix.posenetOK = False
|
||||
else:
|
||||
self.posenet_invalid_count = 0
|
||||
fix.posenetOK = self.posenet_invalid_count < 4
|
||||
fix.posenetOK = True
|
||||
|
||||
#fix.gpsWeek = self.time.week
|
||||
#fix.gpsTimeOfWeek = self.time.tow
|
||||
|
@ -178,15 +188,10 @@ class Localizer():
|
|||
|
||||
def update_kalman(self, time, kind, meas, R=None):
|
||||
try:
|
||||
self.kf.predict_and_observe(time, kind, meas, R=R)
|
||||
self.kf.predict_and_observe(time, kind, meas, 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:
|
||||
# else:
|
||||
# self.observation_buffer.pop(0)
|
||||
|
||||
def handle_gps(self, current_time, log):
|
||||
# ignore the message if the fix is invalid
|
||||
|
@ -244,6 +249,8 @@ class Localizer():
|
|||
trans_device = self.device_from_calib.dot(log.trans)
|
||||
trans_device_std = self.device_from_calib.dot(log.transStd)
|
||||
self.posenet_speed = np.linalg.norm(trans_device)
|
||||
self.posenet_stds[:-1] = self.posenet_stds[1:]
|
||||
self.posenet_stds[-1] = trans_device_std[0]
|
||||
self.update_kalman(current_time,
|
||||
ObservationKind.CAMERA_ODO_TRANSLATION,
|
||||
np.concatenate([trans_device, 10*trans_device_std]))
|
||||
|
@ -322,7 +329,7 @@ def locationd_thread(sm, pm, disabled_logs=None):
|
|||
msg = messaging.new_message('liveLocationKalman')
|
||||
msg.logMonoTime = t
|
||||
|
||||
msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
|
||||
msg.liveLocationKalman = localizer.liveLocationMsg()
|
||||
msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
|
||||
msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents']
|
||||
|
||||
|
|
|
@ -206,7 +206,7 @@ class LiveKalman():
|
|||
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)
|
||||
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, max_rewind_age=0.2)
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
|
|
|
@ -55,11 +55,12 @@ def remove_ignored_fields(msg, ignore):
|
|||
def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=None):
|
||||
if ignore_fields is None:
|
||||
ignore_fields = []
|
||||
|
||||
if ignore_msgs is None:
|
||||
ignore_msgs = []
|
||||
|
||||
log1, log2 = [list(filter(lambda m: m.which() not in ignore_msgs, log)) for log in (log1, log2)]
|
||||
assert len(log1) == len(log2), "logs are not same length: " + str(len(log1)) + " VS " + str(len(log2))
|
||||
if len(log1) != len(log2):
|
||||
raise Exception(f"logs are not same length: {len(log1)} VS {len(log2)}")
|
||||
|
||||
diff = []
|
||||
for msg1, msg2 in tqdm(zip(log1, log2)):
|
||||
|
@ -77,11 +78,12 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non
|
|||
tolerance = EPSILON if tolerance is None else tolerance
|
||||
dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields)
|
||||
|
||||
# Dictiffer only supports relative tolerance, we also want to check for absolute
|
||||
# Dictdiffer only supports relative tolerance, we also want to check for absolute
|
||||
def outside_tolerance(diff):
|
||||
a, b = diff[2]
|
||||
if isinstance(a, numbers.Number) and isinstance(b, numbers.Number):
|
||||
return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b)))
|
||||
if diff[0] == "change":
|
||||
a, b = diff[2]
|
||||
if isinstance(a, numbers.Number) and isinstance(b, numbers.Number):
|
||||
return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b)))
|
||||
return True
|
||||
|
||||
dd = list(filter(outside_tolerance, dd))
|
||||
|
|
|
@ -1 +1 @@
|
|||
f3261f36402ad9bc3003feb9de61a04a634f30e1
|
||||
d8944d6eb13c30c2fd5bcc1af5dbad07472c42c0
|
|
@ -71,8 +71,10 @@ def test_process(cfg, lr, cmp_log_fn, ignore_fields=None, ignore_msgs=None):
|
|||
segment = cmp_log_fn.split("/")[-1].split("_")[0]
|
||||
raise Exception("Route never enabled: %s" % segment)
|
||||
|
||||
return compare_logs(cmp_log_msgs, log_msgs, ignore_fields+cfg.ignore, ignore_msgs, cfg.tolerance)
|
||||
|
||||
try:
|
||||
return compare_logs(cmp_log_msgs, log_msgs, ignore_fields+cfg.ignore, ignore_msgs, cfg.tolerance)
|
||||
except Exception as e:
|
||||
return str(e)
|
||||
|
||||
def format_diff(results, ref_commit):
|
||||
diff1, diff2 = "", ""
|
||||
|
|
Loading…
Reference in New Issue