mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-13 05:12:11 +08:00
Code Issues Packages Projects Releases Wiki Activity

locationd: frequency based bad observation resiliance and recovery (#34476)

Browse Source 
* Improve it

* Fix static

* Fix test_consistent_timing_spikes test

* Fix tests

* Comment

* Remove crap
This commit is contained in:
Kacper Rączy
2025-01-24 21:38:20 -08:00
committed by GitHub
parent 45de7664bc
commit 93a8a18b6d
3 changed files with 39 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.py
+15 -12
View File
@@ -24,14 +24,16 @@ MIN_STD_SANITY_CHECK = 1e-5 # m or rad
MAX_FILTER_REWIND_TIME = 0.8 # s
MAX_SENSOR_TIME_DIFF = 0.1 # s
YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
INPUT_INVALID_THRESHOLD = 0.5 # 0 bad inputs ignored
TIMING_INVALID_THRESHOLD = 2.5 # 2 bad timings ignored
INPUT_INVALID_DECAY = 0.9993 # ~10 secs to resume after exceeding allowed bad inputs by one (at 100hz)
TIMING_INVALID_DECAY = 0.9990 # ~2 secs to resume after exceeding allowed bad timings by one (at 100hz)
INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
POSENET_STD_INITIAL_VALUE = 10.0
POSENET_STD_HIST_HALF = 20
def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency))
def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool):
assert len(values) == len(stds) == 3
measurement.x, measurement.y, measurement.z = map(float, values)
@@ -269,11 +271,11 @@ def main():
filter_initialized = False
critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
observation_timing_invalid = defaultdict(int)
observation_input_invalid = defaultdict(int)
input_invalid_decay = {s: INPUT_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services}
timing_invalid_decay = {s: TIMING_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services}
input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services}
initial_pose = params.get("LocationFilterInitialState")
if initial_pose is not None:
@@ -306,19 +308,20 @@ def main():
continue
if res == HandleLogResult.TIMING_INVALID:
observation_timing_invalid[which] += 1
print(f"Observation {which} ignored due to failed timing check")
observation_input_invalid[which] += 1
print(observation_input_invalid[which])
elif res == HandleLogResult.INPUT_INVALID:
print(f"Observation {which} ignored due to failed sanity check")
observation_input_invalid[which] += 1
else:
observation_input_invalid[which] *= input_invalid_decay[which]
observation_timing_invalid[which] *= timing_invalid_decay[which]
else:
filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
if sm.updated["cameraOdometry"]:
critical_service_inputs_valid = all(observation_input_invalid[s] < INPUT_INVALID_THRESHOLD for s in critcal_services)
critical_service_timing_valid = all(observation_timing_invalid[s] < TIMING_INVALID_THRESHOLD for s in critcal_services)
inputs_valid = sm.all_valid() and critical_service_inputs_valid and critical_service_timing_valid
critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
inputs_valid = sm.all_valid() and critical_service_inputs_valid
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
selfdrive/locationd/test/test_locationd.py
-56
View File
@@ -1,56 +0,0 @@
import capnp
import cereal.messaging as messaging
from openpilot.common.params import Params
from openpilot.system.manager.process_config import managed_processes
class TestLocationdProc:
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
'accelerometer', 'gyroscope', 'magnetometer']
def setup_method(self):
self.pm = messaging.PubMaster(self.LLD_MSGS)
self.params = Params()
self.params.put_bool("UbloxAvailable", True)
managed_processes['locationd'].prepare()
managed_processes['locationd'].start()
def teardown_method(self):
managed_processes['locationd'].stop()
def get_msg(self, name, t):
try:
msg = messaging.new_message(name)
except capnp.lib.capnp.KjException:
msg = messaging.new_message(name, 0)
if name == "gpsLocationExternal":
msg.gpsLocationExternal.flags = 1
msg.gpsLocationExternal.hasFix = True
msg.gpsLocationExternal.verticalAccuracy = 1.0
msg.gpsLocationExternal.speedAccuracy = 1.0
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
msg.gpsLocationExternal.latitude = float(self.lat)
msg.gpsLocationExternal.longitude = float(self.lon)
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
msg.gpsLocationExternal.altitude = float(self.alt)
#if name == "gnssMeasurements":
# msg.gnssMeasurements.measTime = t
# msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
# msg.gnssMeasurements.positionECEF.std = [0,0,0]
# msg.gnssMeasurements.positionECEF.valid = True
# msg.gnssMeasurements.velocityECEF.value = []
# msg.gnssMeasurements.velocityECEF.std = [0,0,0]
# msg.gnssMeasurements.velocityECEF.valid = True
elif name == 'cameraOdometry':
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
msg.logMonoTime = t
msg.valid = True
return msg
selfdrive/locationd/test/test_locationd_scenarios.py
+24 -11
View File
@@ -23,8 +23,10 @@ class Scenario(Enum):
BASE = 'base'
GYRO_OFF = 'gyro_off'
GYRO_SPIKE_MIDWAY = 'gyro_spike_midway'
GYRO_CONSISTENT_SPIKES = 'gyro_consistent_spikes'
ACCEL_OFF = 'accel_off'
ACCEL_SPIKE_MIDWAY = 'accel_spike_midway'
ACCEL_CONSISTENT_SPIKES = 'accel_consistent_spikes'
SENSOR_TIMING_SPIKE_MIDWAY = 'timing_spikes'
SENSOR_TIMING_CONSISTENT_SPIKES = 'timing_consistent_spikes'
@@ -63,18 +65,20 @@ def run_scenarios(scenario, logs):
elif scenario == Scenario.GYRO_OFF:
logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime)
elif scenario == Scenario.GYRO_SPIKE_MIDWAY:
elif scenario == Scenario.GYRO_SPIKE_MIDWAY or scenario == Scenario.GYRO_CONSISTENT_SPIKES:
def gyro_spike(msg):
msg.gyroscope.gyroUncalibrated.v[0] += 3.0
logs = modify_logs_midway(logs, 'gyroscope', 1, gyro_spike)
count = 1 if scenario == Scenario.GYRO_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
logs = modify_logs_midway(logs, 'gyroscope', count, gyro_spike)
elif scenario == Scenario.ACCEL_OFF:
logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime)
elif scenario == Scenario.ACCEL_SPIKE_MIDWAY:
elif scenario == Scenario.ACCEL_SPIKE_MIDWAY or scenario == Scenario.ACCEL_CONSISTENT_SPIKES:
def acc_spike(msg):
msg.accelerometer.acceleration.v[0] += 10.0
logs = modify_logs_midway(logs, 'accelerometer', 1, acc_spike)
msg.accelerometer.acceleration.v[0] += 100.0
count = 1 if scenario == Scenario.ACCEL_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
logs = modify_logs_midway(logs, 'accelerometer', count, acc_spike)
elif scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY or scenario == Scenario.SENSOR_TIMING_CONSISTENT_SPIKES:
def timing_spike(msg):
@@ -121,7 +125,7 @@ class TestLocationdScenarios:
assert np.allclose(replayed_data['roll'], 0.0)
assert np.all(replayed_data['sensors_flag'] == 0.0)
def test_gyro_spikes(self):
def test_gyro_spike(self):
"""
Test: a gyroscope spike in the middle of the segment
Expected Result:
@@ -132,8 +136,17 @@ class TestLocationdScenarios:
orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
assert np.diff(replayed_data['inputs_flag'])[499] == -1.0
assert np.diff(replayed_data['inputs_flag'])[704] == 1.0
assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag'])
assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag'])
def test_consistent_gyro_spikes(self):
"""
Test: consistent timing spikes for N gyroscope messages in the middle of the segment
Expected Result: inputsOK becomes False after N of bad measurements
"""
orig_data, replayed_data = run_scenarios(Scenario.GYRO_CONSISTENT_SPIKES, self.logs)
assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
assert np.diff(replayed_data['inputs_flag'])[708] == 1.0
def test_accel_off(self):
"""
@@ -148,7 +161,7 @@ class TestLocationdScenarios:
assert np.allclose(replayed_data['roll'], 0.0)
assert np.all(replayed_data['sensors_flag'] == 0.0)
def test_accel_spikes(self):
def test_accel_spike(self):
"""
ToDo:
Test: an accelerometer spike in the middle of the segment
@@ -173,5 +186,5 @@ class TestLocationdScenarios:
Expected Result: inputsOK becomes False after N of bad measurements
"""
orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_CONSISTENT_SPIKES, self.logs)
assert np.diff(replayed_data['inputs_flag'])[500] == -1.0
assert np.diff(replayed_data['inputs_flag'])[787] == 1.0
assert np.diff(replayed_data['inputs_flag'])[501] == -1.0
assert np.diff(replayed_data['inputs_flag'])[707] == 1.0