superrob/IQ.Pilot
1
0
Fork 0
forked from IQ.Lvbs/IQ.Pilot
Code Pull Requests Activity

IQ.Pilot Release Commit @ b6534c0

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-08-27 20:17:33 -05:00
commit 00f07cac48
4706 changed files with 1257146 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
iqpilot/selfdrive/locationd/.gitignore vendored Normal file
View File

@@ -0,0 +1,3 @@
params_learner
paramsd
locationd

5
iqpilot/selfdrive/locationd/SConscript Normal file
View File

@@ -0,0 +1,5 @@
Import('env', 'envCython')
native_kernel = env.StaticLibrary("../state_estimation/native_kernels", ["../state_estimation/native_kernels.cc"])
native_binding_env = envCython.Clone(CYTHONFLAGS=["--cplus"])
native_binding_env.Program("../state_estimation/native_binding_pyx.so", ["../state_estimation/native_binding_pyx.pyx"], LIBS=[native_kernel] + envCython["LIBS"])

0
iqpilot/selfdrive/locationd/__init__.py Normal file
View File

32
iqpilot/selfdrive/locationd/calibration_helpers.py Normal file
View File

@@ -0,0 +1,32 @@
import math
import numpy as np
from iqpilot.cereal import log
from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT, HEIGHT_SANE_MIN, HEIGHT_SANE_MAX
def get_calibrated_rpy(live_calib: log.ExtrinsicsCalibration) -> np.ndarray | None:
if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
return None
if len(live_calib.rpyCalib) != 3:
return None
calib_rpy = np.asarray(live_calib.rpyCalib, dtype=np.float32)
return calib_rpy if np.isfinite(calib_rpy).all() else None
def get_render_path_height(live_calib: log.ExtrinsicsCalibration) -> float:
if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
return float(HEIGHT_INIT[0])
if len(live_calib.height) != 1:
return float(HEIGHT_INIT[0])
height = float(live_calib.height[0])
if not math.isfinite(height):
return float(HEIGHT_INIT[0])
if not (HEIGHT_SANE_MIN <= height <= HEIGHT_SANE_MAX):
return float(HEIGHT_INIT[0])
return height

367
iqpilot/selfdrive/locationd/calibrationd.py Executable file
View File

@@ -0,0 +1,367 @@
#!/usr/bin/env python3
'''
This process finds calibration values. More info on what these calibration values
are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations
While the roll calibration is a real value that can be estimated, here we assume it's zero,
and the image input into the neural network is not corrected for roll.
'''
import os
import capnp
import numpy as np
from typing import NoReturn
from iqpilot.cereal import log, car
import iqpilot.cereal.messaging as messaging
from iqpilot.common.constants import CV
from iqpilot.common.params import Params
from iqpilot.common.issue_debug import log_issue_limited
from iqpilot.common.realtime import config_realtime_process
from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
from iqpilot.common.swaglog import cloudlog
from iqpilot.system.hardware import HARDWARE
MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
MAX_VEL_ANGLE_STD = np.radians(0.25)
MAX_YAW_RATE_FILTER = np.radians(2) # per second
MAX_HEIGHT_STD = np.exp(-3.5)
# This is at model frequency, blocks needed for efficiency
SMOOTH_CYCLES = 10
BLOCK_SIZE = 100
INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration
INPUTS_WANTED = 50 # We want a little bit more than we need for stability
MAX_ALLOWED_YAW_SPREAD = np.radians(2)
MAX_ALLOWED_PITCH_SPREAD = np.radians(4)
TICI_FAMILY_PITCH_SPREAD_RESET = np.radians(3)
RPY_INIT = np.array([0.0,0.0,0.0])
WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0])
HEIGHT_INIT = np.array([1.22])
HEIGHT_SANE_MIN, HEIGHT_SANE_MAX = 0.9, 2.0
DEVICE_IS_TICI_FAMILY = HARDWARE.get_device_type() in ("tici", "tizi")
# These values are needed to accommodate the model frame in the narrow cam
if HARDWARE.get_device_type() == 'mici':
PITCH_LIMITS = np.array([-0.143101, 0.22235988])
else:
PITCH_LIMITS = np.array([-0.09074112085129739, 0.17])
YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
DEBUG = os.getenv("DEBUG") is not None
def is_calibration_valid(rpy: np.ndarray) -> bool:
return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
def sanity_clip(rpy: np.ndarray) -> np.ndarray:
if np.isnan(rpy).any():
rpy = RPY_INIT
return np.array([rpy[0],
np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005),
np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx: int, block_size: float) -> np.ndarray:
return (idx*prev_mean + (block_size - idx) * new_val) / block_size
class Calibrator:
def __init__(self, param_put: bool = False):
self.param_put = param_put
self.not_car = False
self.stable_rpy = RPY_INIT.copy()
self.stable_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
self.stable_height = HEIGHT_INIT.copy()
self.has_stable_snapshot = False
# Read saved calibration
self.params = Params()
calibration_params = self.params.get("CalibrationParams")
rpy_init = RPY_INIT
wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
height = HEIGHT_INIT
valid_blocks = 0
self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
if param_put and calibration_params:
try:
with log.Event.from_bytes(calibration_params) as msg:
rpy_init = np.array(msg.extrinsicsCalibration.rpyCalib)
valid_blocks = msg.extrinsicsCalibration.validBlocks
wide_from_device_euler = np.array(msg.extrinsicsCalibration.wideFromDeviceEuler)
height = np.array(msg.extrinsicsCalibration.height)
except Exception:
cloudlog.exception("Error reading cached CalibrationParams")
self.reset(rpy_init, valid_blocks, wide_from_device_euler, height)
self.update_status()
# If saved calibration is immediately invalid (e.g. bad params from a previous
# bootstrap bug or device remount), auto-clear it so we recalibrate from scratch
# instead of getting permanently stuck in the "Calibration Invalid" state.
if self.cal_status == log.ExtrinsicsCalibration.Status.invalid:
cloudlog.warning("calibrationd: saved CalibrationParams are invalid, clearing and starting fresh")
if param_put:
self.params.remove("CalibrationParams")
self.reset()
self.update_status()
def _remember_stable_solution(self) -> None:
self.stable_rpy = self.rpy.copy()
self.stable_wide_from_device_euler = self.wide_from_device_euler.copy()
self.stable_height = self.height.copy()
self.has_stable_snapshot = True
def reset(self, rpy_init: np.ndarray = RPY_INIT,
valid_blocks: int = 0,
wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
height_init: np.ndarray = HEIGHT_INIT,
smooth_from: np.ndarray | None = None) -> None:
if not np.isfinite(rpy_init).all():
self.rpy = RPY_INIT.copy()
else:
self.rpy = rpy_init.copy()
if not np.isfinite(height_init).all() or len(height_init) != 1:
self.height = HEIGHT_INIT.copy()
else:
self.height = height_init.copy()
if not np.isfinite(wide_from_device_euler_init).all() or len(wide_from_device_euler_init) != 3:
self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
else:
self.wide_from_device_euler = wide_from_device_euler_init.copy()
if not np.isfinite(valid_blocks) or valid_blocks < 0:
self.valid_blocks = 0
else:
self.valid_blocks = valid_blocks
self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1))
self.wide_from_device_eulers = np.tile(self.wide_from_device_euler, (INPUTS_WANTED, 1))
self.heights = np.tile(self.height, (INPUTS_WANTED, 1))
self.idx = 0
self.block_idx = 0
self.v_ego = 0.0
if smooth_from is None:
self.old_rpy = RPY_INIT
self.old_rpy_weight = 0.0
else:
self.old_rpy = smooth_from
self.old_rpy_weight = 1.0
def get_valid_idxs(self) -> list[int]:
# exclude current block_idx from validity window
before_current = list(range(self.block_idx))
after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks))
return before_current + after_current
def update_status(self) -> None:
valid_idxs = self.get_valid_idxs()
if valid_idxs:
self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0)
self.height = np.mean(self.heights[valid_idxs], axis=0)
rpys = self.rpys[valid_idxs]
self.rpy = np.mean(rpys, axis=0)
max_rpy_calib = np.array(np.max(rpys, axis=0))
min_rpy_calib = np.array(np.min(rpys, axis=0))
self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
else:
self.calib_spread = np.zeros(3)
if self.valid_blocks < INPUTS_NEEDED:
if self.cal_status == log.ExtrinsicsCalibration.Status.recalibrating:
self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
else:
self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
elif is_calibration_valid(self.rpy):
self.cal_status = log.ExtrinsicsCalibration.Status.calibrated
else:
self.cal_status = log.ExtrinsicsCalibration.Status.invalid
# If spread is too high, assume mounting was changed and reset to last block.
# Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
# TODO: add height spread check with smooth transition too
pitch_spread_limit = TICI_FAMILY_PITCH_SPREAD_RESET if DEVICE_IS_TICI_FAMILY else MAX_ALLOWED_PITCH_SPREAD
spread_too_high = self.calib_spread[1] > pitch_spread_limit or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD
if self.cal_status == log.ExtrinsicsCalibration.Status.calibrated and not spread_too_high:
self._remember_stable_solution()
if spread_too_high and self.cal_status == log.ExtrinsicsCalibration.Status.calibrated:
use_stable_snapshot = DEVICE_IS_TICI_FAMILY and self.has_stable_snapshot
if use_stable_snapshot:
reset_rpy = self.stable_rpy
reset_wide = self.stable_wide_from_device_euler
reset_height = self.stable_height
else:
reset_rpy = self.rpys[self.block_idx - 1]
reset_wide = self.wide_from_device_eulers[self.block_idx - 1]
reset_height = self.heights[self.block_idx - 1]
log_issue_limited(
"calibrationd_reset_spread",
"calibration",
f"calibrationd reset unstable solution pitchSpread={self.calib_spread[1]:.6f} "
f"yawSpread={self.calib_spread[2]:.6f} pitchLimit={pitch_spread_limit:.6f} "
f"use_stable_snapshot={use_stable_snapshot} rpy={self.rpy.tolist()}",
interval_sec=0.5,
)
self.reset(reset_rpy, valid_blocks=1, wide_from_device_euler_init=reset_wide,
height_init=reset_height, smooth_from=self.stable_rpy if use_stable_snapshot else self.rpy)
self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
if self.param_put and write_this_cycle:
self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes())
def handle_v_ego(self, v_ego: float) -> None:
self.v_ego = v_ego
def get_smooth_rpy(self) -> np.ndarray:
if self.old_rpy_weight > 0:
return self.old_rpy_weight * self.old_rpy + (1.0 - self.old_rpy_weight) * self.rpy
else:
return self.rpy
def handle_cam_odom(self, trans: list[float],
rot: list[float],
wide_from_device_euler: list[float],
trans_std: list[float],
road_transform_trans: list[float],
road_transform_trans_std: list[float]) -> np.ndarray | None:
self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
fast_enough = self.v_ego > MIN_SPEED_FILTER
motion_speed = max(float(self.v_ego), float(trans[0]))
cam_fast_enough = motion_speed > MIN_SPEED_FILTER
yaw_ok = abs(rot[2]) < MAX_YAW_RATE_FILTER
straight_and_fast = fast_enough and cam_fast_enough and yaw_ok
angle_std_threshold = MAX_VEL_ANGLE_STD
height_std_threshold = MAX_HEIGHT_STD
rpy_certain = np.arctan2(trans_std[1], motion_speed) < angle_std_threshold
if len(road_transform_trans_std) == 3:
height_certain = road_transform_trans_std[2] < height_std_threshold
else:
height_certain = True
certain_if_calib = rpy_certain
if not (straight_and_fast and certain_if_calib):
log_issue_limited(
"calibrationd_rejected_sample",
"calibration",
f"calibrationd rejected sample vEgo={self.v_ego:.2f} trans0={trans[0]:.2f} yawRate={rot[2]:.4f} "
f"fast_enough={fast_enough} cam_fast_enough={cam_fast_enough} motion_speed={motion_speed:.2f} yaw_ok={yaw_ok} "
f"rpy_certain={rpy_certain} height_certain={height_certain} valid_blocks={self.valid_blocks} idx={self.idx}",
interval_sec=1.0,
)
return None
observed_rpy = np.array([0,
-np.arctan2(trans[2], trans[0]),
np.arctan2(trans[1], trans[0])])
new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy)))
new_rpy = sanity_clip(new_rpy)
if len(wide_from_device_euler) == 3:
new_wide_from_device_euler = np.array(wide_from_device_euler)
else:
new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
if len(road_transform_trans) == 3 and HEIGHT_SANE_MIN <= road_transform_trans[2] <= HEIGHT_SANE_MAX:
new_height = np.array([road_transform_trans[2]])
else:
new_height = HEIGHT_INIT
self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE))
self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx],
new_wide_from_device_euler, self.idx, float(BLOCK_SIZE))
self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE))
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
self.update_status()
if self.idx == 0:
log_issue_limited(
"calibrationd_progress_block",
"calibration",
f"calibrationd progress status={int(self.cal_status)} valid_blocks={self.valid_blocks} "
f"calPerc={min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)} "
f"rpy={self.rpy.tolist()} spread={self.calib_spread.tolist()}",
interval_sec=0.5,
)
return new_rpy
def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder:
smooth_rpy = self.get_smooth_rpy()
msg = messaging.new_message('extrinsicsCalibration')
msg.valid = valid
extrinsicsCalibration = msg.extrinsicsCalibration
extrinsicsCalibration.validBlocks = self.valid_blocks
extrinsicsCalibration.calStatus = self.cal_status
extrinsicsCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
extrinsicsCalibration.rpyCalib = smooth_rpy.tolist()
extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist()
extrinsicsCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
extrinsicsCalibration.height = self.height.tolist()
return msg
def send_data(self, pm: messaging.PubMaster, valid: bool) -> None:
pm.send('extrinsicsCalibration', self.get_msg(valid))
def main() -> NoReturn:
config_realtime_process([0, 1, 2, 3], 5)
pm = messaging.PubMaster(['extrinsicsCalibration'])
sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry')
params_reader = Params()
CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
calibrator = Calibrator(param_put=True)
calibrator.not_car = CP.notCar
while 1:
timeout = 0 if sm.frame == -1 else 100
sm.update(timeout)
if sm.updated['cameraOdometry']:
calibrator.handle_v_ego(sm['carState'].vEgo)
new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
sm['cameraOdometry'].rot,
sm['cameraOdometry'].wideFromDeviceEuler,
sm['cameraOdometry'].transStd,
sm['cameraOdometry'].roadTransformTrans,
sm['cameraOdometry'].roadTransformTransStd)
if DEBUG and new_rpy is not None:
print('got new rpy', new_rpy)
# 4Hz driven by cameraOdometry
if sm.frame % 5 == 0:
checks_ok = sm.all_checks()
if not checks_ok:
ft = sm.freq_tracker
recv_hz = {s: (round(1.0 / ft[s].avg_dt.get_average(), 2) if ft[s].avg_dt.count else None) for s in sm.services}
log_issue_limited(
"calibrationd_checks_failed",
"calibration",
f"calibrationd all_checks failed alive={sm.alive} freq_ok={sm.freq_ok} valid={sm.valid} "
f"seen={sm.seen} recv_hz={recv_hz}",
interval_sec=5.0,
)
calibrator.send_data(pm, checks_ok)
if __name__ == "__main__":
main()

103
iqpilot/selfdrive/locationd/estimatord.py Executable file
View File

@@ -0,0 +1,103 @@
#!/usr/bin/env python3
import os
import numpy as np
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import car
from iqpilot.cereal.services import SERVICE_LIST
from iqpilot.common.params import Params
from iqpilot.common.realtime import config_realtime_process
from iqpilot.common.steer_delay import SteerDelayPublisher
from iqpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag
from iqpilot.selfdrive.locationd.paramsd import (
VehicleParamsEstimator,
migrate_cached_vehicle_params_if_needed,
retrieve_initial_vehicle_params,
)
from iqpilot.selfdrive.locationd.torqued import TorqueEstimator
PARAMS_SERVICES = ['deviceMotion', 'extrinsicsCalibration', 'carState']
LAG_SERVICES = ['deviceMotion', 'extrinsicsCalibration', 'carState', 'controlsState', 'carControl']
TORQUE_SERVICES = ['carControl', 'carOutput', 'carState', 'extrinsicsCalibration', 'deviceMotion']
SUBSCRIBED_SERVICES = list(dict.fromkeys(PARAMS_SERVICES + LAG_SERVICES + TORQUE_SERVICES))
def main() -> None:
config_realtime_process([0, 1, 2, 3], 5)
debug = bool(int(os.getenv('DEBUG', '0')))
replay = bool(int(os.getenv('REPLAY', '0')))
pm = messaging.PubMaster(['vehicleParameters', 'lateralDelay', 'lateralTorqueParameters'])
sm = messaging.SubMaster(SUBSCRIBED_SERVICES, poll='deviceMotion')
params = Params()
CP = messaging.log_from_bytes(params.get('CarParams', block=True), car.CarParams)
migrate_cached_vehicle_params_if_needed(params)
steer_ratio, stiffness_factor, angle_offset_deg, p_initial = retrieve_initial_vehicle_params(params, CP, replay, debug)
params_estimator = VehicleParamsEstimator(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), p_initial)
lag_estimator = LateralLagEstimator(CP, 1. / SERVICE_LIST['deviceMotion'].frequency)
if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
lag, valid_blocks = initial_lag_params
lag_estimator.reset(lag, valid_blocks)
torque_estimator = TorqueEstimator(CP)
steer_delay_publisher = SteerDelayPublisher(CP)
estimators = (
(params_estimator, PARAMS_SERVICES),
(lag_estimator, LAG_SERVICES),
(torque_estimator, TORQUE_SERVICES),
)
while True:
sm.update()
valid = sm.all_checks()
if valid:
for which in sorted(sm.updated, key=lambda x: sm.logMonoTime[x]):
if not sm.updated[which]:
continue
t = sm.logMonoTime[which] * 1e-9
for estimator, services in estimators:
if which in services:
estimator.handle_log(t, which, sm[which])
lag_estimator.update_points()
if not sm.updated['deviceMotion']:
continue
params_msg = params_estimator.get_msg(valid, debug=debug)
params_msg_dat = params_msg.to_bytes()
if sm.frame % 1200 == 0:
params.put_nonblocking('LiveParametersV2', params_msg_dat)
pm.send('vehicleParameters', params_msg_dat)
if sm.frame % 5 != 0:
continue
lag_estimator.update_estimate()
lag_msg = lag_estimator.get_msg(valid, debug)
lag_msg_dat = lag_msg.to_bytes()
pm.send('lateralDelay', lag_msg_dat)
torque_estimator.handle_log(sm.logMonoTime['deviceMotion'] * 1e-9, 'lateralDelay', lag_msg.lateralDelay)
pm.send('lateralTorqueParameters', torque_estimator.get_msg(valid=valid, with_points=debug))
if sm.frame % 1200 == 0:
params.put_nonblocking('LiveDelay', lag_msg_dat)
if sm.frame % 60 == 0:
steer_delay_publisher.update(lag_msg)
if sm.frame % 240 == 0:
torque_msg = torque_estimator.get_msg(valid=valid, with_points=True)
params.put_nonblocking('LiveTorqueParameters', torque_msg.to_bytes())
if __name__ == '__main__':
main()

188
iqpilot/selfdrive/locationd/helpers.py Normal file
View File

@@ -0,0 +1,188 @@
import numpy as np
from typing import Any
from functools import cache
from iqpilot.cereal import log
from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
@cache
def fft_next_good_size(n: int) -> int:
"""
smallest composite of 2, 3, 5, 7, 11 that is >= n
inspired by pocketfft
"""
if n <= 6:
return n
best, f2 = 2 * n, 1
while f2 < best:
f23 = f2
while f23 < best:
f235 = f23
while f235 < best:
f2357 = f235
while f2357 < best:
f235711 = f2357
while f235711 < best:
best = f235711 if f235711 >= n else best
f235711 *= 11
f2357 *= 7
f235 *= 5
f23 *= 3
f2 *= 2
return best
def parabolic_peak_interp(R, max_index):
if max_index == 0 or max_index == len(R) - 1:
return max_index
y_m1, y_0, y_p1 = R[max_index - 1], R[max_index], R[max_index + 1]
offset = 0.5 * (y_p1 - y_m1) / (2 * y_0 - y_p1 - y_m1)
return max_index + offset
def rotate_cov(rot_matrix, cov_in):
return rot_matrix @ cov_in @ rot_matrix.T
def rotate_std(rot_matrix, std_in):
return np.sqrt(np.diag(rotate_cov(rot_matrix, np.diag(std_in**2))))
class NPQueue:
def __init__(self, maxlen: int, rowsize: int) -> None:
self.maxlen = maxlen
self.arr = np.empty((0, rowsize))
def __len__(self) -> int:
return len(self.arr)
def append(self, pt: list[float]) -> None:
if len(self.arr) < self.maxlen:
self.arr = np.append(self.arr, [pt], axis=0)
else:
self.arr[:-1] = self.arr[1:]
self.arr[-1] = pt
class PointBuckets:
def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None:
self.x_bounds = x_bounds
self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds}
self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True))
self.min_points_total = min_points_total
def __len__(self) -> int:
return sum([len(v) for v in self.buckets.values()])
def is_valid(self) -> bool:
individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True))
total_points_valid = self.__len__() >= self.min_points_total
return individual_buckets_valid and total_points_valid
def get_valid_percent(self) -> int:
total_points_perc = min(self.__len__() / self.min_points_total * 100, 100)
individual_buckets_perc = min(min(len(v) / min_pts * 100 for v, min_pts in
zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)), 100)
return int((total_points_perc + individual_buckets_perc) / 2)
def is_calculable(self) -> bool:
return all(len(v) > 0 for v in self.buckets.values())
def add_point(self, x: float, y: float) -> None:
raise NotImplementedError
def get_points(self, num_points: int | None = None) -> Any:
points = np.vstack([x.arr for x in self.buckets.values()])
if num_points is None:
return points
return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
def load_points(self, points: list[list[float]]) -> None:
for point in points:
self.add_point(*point)
class ParameterEstimator:
""" Base class for parameter estimators """
def reset(self) -> None:
raise NotImplementedError
def handle_log(self, t: int, which: str, msg: log.Event) -> None:
raise NotImplementedError
def get_msg(self, valid: bool, with_points: bool) -> log.Event:
raise NotImplementedError
class Measurement:
x, y, z = (property(lambda self: self.xyz[0]), property(lambda self: self.xyz[1]), property(lambda self: self.xyz[2]))
x_std, y_std, z_std = (property(lambda self: self.xyz_std[0]), property(lambda self: self.xyz_std[1]), property(lambda self: self.xyz_std[2]))
roll, pitch, yaw = x, y, z
roll_std, pitch_std, yaw_std = x_std, y_std, z_std
def __init__(self, xyz: np.ndarray, xyz_std: np.ndarray):
self.xyz: np.ndarray = xyz
self.xyz_std: np.ndarray = xyz_std
@classmethod
def from_measurement_xyz(cls, measurement: log.DeviceMotion.XYZMeasurement) -> 'Measurement':
return cls(
xyz=np.array([measurement.x, measurement.y, measurement.z]),
xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd])
)
class Pose:
def __init__(self, orientation: Measurement, velocity: Measurement, acceleration: Measurement, angular_velocity: Measurement):
self.orientation = orientation
self.velocity = velocity
self.acceleration = acceleration
self.angular_velocity = angular_velocity
@classmethod
def from_live_pose(cls, live_pose: log.DeviceMotion) -> 'Pose':
return Pose(
orientation=Measurement.from_measurement_xyz(live_pose.orientationNED),
velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice),
acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice),
angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice)
)
class PoseCalibrator:
def __init__(self):
self.calib_valid = False
self.calib_from_device = np.eye(3)
def _transform_calib_from_device(self, meas: Measurement):
new_xyz = self.calib_from_device @ meas.xyz
new_xyz_std = rotate_std(self.calib_from_device, meas.xyz_std)
return Measurement(new_xyz, new_xyz_std)
def _ned_from_calib(self, orientation: Measurement):
ned_from_device = rot_from_euler(orientation.xyz)
ned_from_calib = ned_from_device @ self.calib_from_device.T
ned_from_calib_euler_meas = Measurement(euler_from_rot(ned_from_calib), np.full(3, np.nan))
return ned_from_calib_euler_meas
def build_calibrated_pose(self, pose: Pose) -> Pose:
ned_from_calib_euler = self._ned_from_calib(pose.orientation)
angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity)
acceleration_calib = self._transform_calib_from_device(pose.acceleration)
velocity_calib = self._transform_calib_from_device(pose.velocity)
return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib)
def feed_live_calib(self, live_calib: log.ExtrinsicsCalibration):
calib_rpy = get_calibrated_rpy(live_calib)
if calib_rpy is not None:
self.calib_from_device = rot_from_euler(calib_rpy).T
self.calib_valid = True
else:
if not self.calib_valid:
self.calib_from_device = np.eye(3)
self.calib_valid = False

381
iqpilot/selfdrive/locationd/lagd.py Executable file
View File

@@ -0,0 +1,381 @@
#!/usr/bin/env python3
import numpy as np
import capnp
from collections import deque
from functools import partial
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import car, log
from iqpilot.common.params import Params
from iqpilot.common.constants import CV
from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp
BLOCK_SIZE = 100
BLOCK_NUM = 50
BLOCK_NUM_NEEDED = 5
MOVING_WINDOW_SEC = 60.0
MIN_OKAY_WINDOW_SEC = 25.0
MIN_RECOVERY_BUFFER_SEC = 2.0
MIN_VEGO = 50.0 * CV.MPH_TO_MS
MIN_ABS_YAW_RATE = 0.0
MAX_YAW_RATE_SANITY_CHECK = 1.0
MIN_NCC = 0.95
MAX_LAG = 0.65
MIN_LAG = 0.15
MAX_LAG_STD = 0.1
MAX_LAT_ACCEL = 2.0
MAX_LAT_ACCEL_DIFF = 0.6
MIN_LAT_ACCEL_RANGE = 0.5
MIN_CONFIDENCE = 0.7
CORR_BORDER_OFFSET = 5
LAG_CANDIDATE_CORR_THRESHOLD = 0.9
SMOOTH_K = 5
SMOOTH_SIGMA = 1.0
VERSION = 1 # bump this to invalidate old parameter caches
def masked_symmetric_moving_average(x: np.ndarray, mask: np.ndarray, k: int, sigma: float) -> np.ndarray:
assert k >= 1 and k % 2 == 1, "k must be positive and odd"
pad = k // 2
i = np.arange(k) - pad
w = np.exp(-0.5 * (i / sigma) ** 2)
w /= w.sum()
xp = np.pad(x * mask, pad, mode="edge")
mp = np.pad(mask, pad, mode="edge")
num = np.convolve(xp, w, mode="valid")
den = np.convolve(mp, w, mode="valid")
return np.divide(num, den, out=np.full_like(num, np.nan, dtype=np.float64), where=den != 0)
def masked_normalized_cross_correlation(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, n: int):
"""
References:
D. Padfield. "Masked FFT registration". In Proc. Computer Vision and
Pattern Recognition, pp. 2918-2925 (2010).
:DOI:`10.1109/CVPR.2010.5540032`
"""
eps = np.finfo(np.float64).eps
expected_sig = np.asarray(expected_sig, dtype=np.float64)
actual_sig = np.asarray(actual_sig, dtype=np.float64)
expected_sig[~mask] = 0.0
actual_sig[~mask] = 0.0
rotated_expected_sig = expected_sig[::-1]
rotated_mask = mask[::-1]
fft = partial(np.fft.fft, n=n)
actual_sig_fft = fft(actual_sig)
rotated_expected_sig_fft = fft(rotated_expected_sig)
actual_mask_fft = fft(mask.astype(np.float64))
rotated_mask_fft = fft(rotated_mask.astype(np.float64))
number_overlap_masked_samples = np.fft.ifft(rotated_mask_fft * actual_mask_fft).real
number_overlap_masked_samples[:] = np.round(number_overlap_masked_samples)
number_overlap_masked_samples[:] = np.fmax(number_overlap_masked_samples, eps)
masked_correlated_actual_fft = np.fft.ifft(rotated_mask_fft * actual_sig_fft).real
masked_correlated_expected_fft = np.fft.ifft(actual_mask_fft * rotated_expected_sig_fft).real
numerator = np.fft.ifft(rotated_expected_sig_fft * actual_sig_fft).real
numerator -= masked_correlated_actual_fft * masked_correlated_expected_fft / number_overlap_masked_samples
actual_squared_fft = fft(actual_sig ** 2)
actual_sig_denom = np.fft.ifft(rotated_mask_fft * actual_squared_fft).real
actual_sig_denom -= masked_correlated_actual_fft ** 2 / number_overlap_masked_samples
actual_sig_denom[:] = np.fmax(actual_sig_denom, 0.0)
rotated_expected_squared_fft = fft(rotated_expected_sig ** 2)
expected_sig_denom = np.fft.ifft(actual_mask_fft * rotated_expected_squared_fft).real
expected_sig_denom -= masked_correlated_expected_fft ** 2 / number_overlap_masked_samples
expected_sig_denom[:] = np.fmax(expected_sig_denom, 0.0)
denom = np.sqrt(actual_sig_denom * expected_sig_denom)
# zero-out samples with very small denominators
tol = 1e3 * eps * np.max(np.abs(denom), keepdims=True)
nonzero_indices = denom > tol
ncc = np.zeros_like(denom, dtype=np.float64)
ncc[nonzero_indices] = numerator[nonzero_indices] / denom[nonzero_indices]
np.clip(ncc, -1, 1, out=ncc)
return ncc
class Points:
def __init__(self, num_points: int):
self.times = deque[float]([0.0] * num_points, maxlen=num_points)
self.okay = deque[bool]([False] * num_points, maxlen=num_points)
self.desired = deque[float]([0.0] * num_points, maxlen=num_points)
self.actual = deque[float]([0.0] * num_points, maxlen=num_points)
@property
def num_points(self):
return len(self.desired)
@property
def num_okay(self):
return np.count_nonzero(self.okay)
def update(self, t: float, desired: float, actual: float, okay: bool):
self.times.append(t)
self.okay.append(okay)
self.desired.append(desired)
self.actual.append(actual)
def get(self) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
return np.array(self.times), np.array(self.desired), np.array(self.actual), np.array(self.okay)
class BlockAverage:
def __init__(self, num_blocks: int, block_size: int, valid_blocks: int, initial_value: float):
self.num_blocks = num_blocks
self.block_size = block_size
self.block_idx = valid_blocks % num_blocks
self.idx = 0
self.values = np.tile(initial_value, (num_blocks, 1))
self.valid_blocks = valid_blocks
def update(self, value: float):
self.values[self.block_idx] = (self.idx * self.values[self.block_idx] + value) / (self.idx + 1)
self.idx = (self.idx + 1) % self.block_size
if self.idx == 0:
self.block_idx = (self.block_idx + 1) % self.num_blocks
self.valid_blocks = min(self.valid_blocks + 1, self.num_blocks)
def get(self) -> tuple[float, float, float, float]:
valid_block_idx = [i for i in range(self.valid_blocks) if i != self.block_idx]
valid_and_current_idx = valid_block_idx + ([self.block_idx] if self.idx > 0 else [])
if len(valid_block_idx) > 0:
valid_mean = float(np.mean(self.values[valid_block_idx], axis=0).item())
valid_std = float(np.std(self.values[valid_block_idx], axis=0).item())
else:
valid_mean, valid_std = float('nan'), float('nan')
if len(valid_and_current_idx) > 0:
current_mean = float(np.mean(self.values[valid_and_current_idx], axis=0).item())
current_std = float(np.std(self.values[valid_and_current_idx], axis=0).item())
else:
current_mean, current_std = float('nan'), float('nan')
return valid_mean, valid_std, current_mean, current_std
class LateralLagEstimator:
inputs = {"carControl", "carState", "controlsState", "extrinsicsCalibration", "deviceMotion"}
def __init__(self, CP: car.CarParams, dt: float,
block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE,
window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC,
min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC,
max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE):
self.dt = dt
self.window_sec = window_sec
self.okay_window_sec = okay_window_sec
self.min_recovery_buffer_sec = min_recovery_buffer_sec
self.initial_lag = CP.steerActuatorDelay + 0.2
self.block_size = block_size
self.block_count = block_count
self.min_valid_block_count = min_valid_block_count
self.min_vego = min_vego
self.min_yr = min_yr
self.min_ncc = min_ncc
self.min_confidence = min_confidence
self.max_lat_accel = max_lat_accel
self.max_lat_accel_diff = max_lat_accel_diff
self.t = 0.0
self.lat_active = False
self.steering_pressed = False
self.steering_saturated = False
self.desired_curvature = 0.0
self.v_ego = 0.0
self.yaw_rate = 0.0
self.yaw_rate_std = 0.0
self.pose_valid = False
self.last_lat_inactive_t = 0.0
self.last_steering_pressed_t = 0.0
self.last_steering_saturated_t = 0.0
self.last_pose_invalid_t = 0.0
self.last_estimate_t = 0.0
self.calibrator = PoseCalibrator()
self.reset(self.initial_lag, 0)
def reset(self, initial_lag: float, valid_blocks: int):
window_len = int(self.window_sec / self.dt)
self.points = Points(window_len)
self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag)
def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
msg = messaging.new_message('lateralDelay')
msg.valid = valid
lateralDelay = msg.lateralDelay
valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get()
if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std):
if valid_std > MAX_LAG_STD:
lateralDelay.status = log.LateralDelay.Status.invalid
else:
lateralDelay.status = log.LateralDelay.Status.estimated
else:
lateralDelay.status = log.LateralDelay.Status.unestimated
if lateralDelay.status == log.LateralDelay.Status.estimated:
lateralDelay.lateralDelay = min(MAX_LAG, max(MIN_LAG, valid_mean_lag))
else:
lateralDelay.lateralDelay = self.initial_lag
if not np.isnan(current_mean_lag) and not np.isnan(current_std):
lateralDelay.lateralDelayEstimate = current_mean_lag
lateralDelay.lateralDelayEstimateStd = current_std
else:
lateralDelay.lateralDelayEstimate = self.initial_lag
lateralDelay.lateralDelayEstimateStd = 0.0
lateralDelay.validBlocks = self.block_avg.valid_blocks
lateralDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) //
(self.min_valid_block_count * self.block_size), 100)
if debug:
lateralDelay.points = self.block_avg.values.flatten().tolist()
lateralDelay.version = VERSION
return msg
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
if which == "carControl":
self.lat_active = msg.latActive
elif which == "carState":
self.steering_pressed = msg.steeringPressed
self.v_ego = msg.vEgo
elif which == "controlsState":
self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated
self.desired_curvature = msg.desiredCurvature
elif which == "extrinsicsCalibration":
self.calibrator.feed_live_calib(msg)
elif which == "deviceMotion":
device_pose = Pose.from_live_pose(msg)
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
self.yaw_rate = calibrated_pose.angular_velocity.yaw
self.yaw_rate_std = calibrated_pose.angular_velocity.yaw_std
self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK
self.t = t
def points_enough(self):
return self.points.num_points >= int(self.okay_window_sec / self.dt)
def points_valid(self):
return self.points.num_okay >= int(self.okay_window_sec / self.dt)
def update_points(self):
la_desired = self.desired_curvature * self.v_ego * self.v_ego
la_actual_pose = self.yaw_rate * self.v_ego
fast = self.v_ego > self.min_vego
turning = np.abs(self.yaw_rate) >= self.min_yr
sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK
la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff
calib_valid = self.calibrator.calib_valid
if not self.lat_active:
self.last_lat_inactive_t = self.t
if self.steering_pressed:
self.last_steering_pressed_t = self.t
if self.steering_saturated:
self.last_steering_saturated_t = self.t
if not sensors_valid or not la_valid:
self.last_pose_invalid_t = self.t
has_recovered = all( # wait for recovery after !lat_active, steering_pressed, steering_saturated, !sensors/la_valid
self.t - last_t >= self.min_recovery_buffer_sec
for last_t in [self.last_lat_inactive_t, self.last_steering_pressed_t, self.last_steering_saturated_t, self.last_pose_invalid_t]
)
okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and \
fast and turning and has_recovered and calib_valid and sensors_valid and la_valid
self.points.update(self.t, la_desired, la_actual_pose, okay)
def update_estimate(self):
if not self.points_enough():
return
times, desired, actual, okay = self.points.get()
# check if there are any new valid data points since the last update
is_valid = self.points_valid() and (actual.max() - actual.min() >= MIN_LAT_ACCEL_RANGE)
if self.last_estimate_t != 0 and times[0] <= self.last_estimate_t:
new_values_start_idx = next(-i for i, t in enumerate(reversed(times)) if t <= self.last_estimate_t)
is_valid = is_valid and not (new_values_start_idx == 0 or not np.any(okay[new_values_start_idx:]))
desired = masked_symmetric_moving_average(desired, okay, SMOOTH_K, SMOOTH_SIGMA)
actual = masked_symmetric_moving_average(actual, okay, SMOOTH_K, SMOOTH_SIGMA)
delay, corr, confidence = self.actuator_delay(desired, actual, okay, self.dt, MIN_LAG, MAX_LAG)
if corr < self.min_ncc or confidence < self.min_confidence or not is_valid:
return
self.block_avg.update(delay)
self.last_estimate_t = self.t
@staticmethod
def actuator_delay(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray,
dt: float, min_lag: float, max_lag: float) -> tuple[float, float, float]:
assert len(expected_sig) == len(actual_sig)
min_lag_samples, max_lag_samples, one_sec_samples = int(round(min_lag / dt)), int(round(max_lag / dt)), int(round(1.0 / dt))
padded_size = fft_next_good_size(len(expected_sig) + max(max_lag_samples, one_sec_samples))
ncc = masked_normalized_cross_correlation(expected_sig, actual_sig, mask, padded_size)
# only consider lags from ranges:
roi = np.s_[len(expected_sig) - 1 + min_lag_samples: len(expected_sig) - 1 + max_lag_samples] # min_lag - max_lag range
threshold_roi = np.s_[len(expected_sig) - 1: len(expected_sig) - 1 + one_sec_samples] # 0 - 1 second range
confidence_roi = np.s_[threshold_roi.start - CORR_BORDER_OFFSET: threshold_roi.stop + CORR_BORDER_OFFSET] # threshold range +/- border
roi_ncc, confidence_roi_ncc, threshold_roi_ncc = ncc[roi], ncc[confidence_roi], ncc[threshold_roi]
max_corr_index = np.argmax(roi_ncc)
corr = roi_ncc[max_corr_index]
lag = parabolic_peak_interp(roi_ncc, max_corr_index) * dt + min_lag
# to estimate lag confidence, gather all high-correlation candidates and see how spread they are
# if e.g. 0.8 and 0.4 are both viable, this is an ambiguous case
ncc_thresh = (threshold_roi_ncc.max() - threshold_roi_ncc.min()) * LAG_CANDIDATE_CORR_THRESHOLD + threshold_roi_ncc.min()
good_lag_candidate_mask = confidence_roi_ncc >= ncc_thresh
good_lag_candidate_edges = np.diff(good_lag_candidate_mask.astype(int), prepend=0, append=0)
starts, ends = np.where(good_lag_candidate_edges == 1)[0], np.where(good_lag_candidate_edges == -1)[0] - 1
run_idx = np.searchsorted(starts, max_corr_index + CORR_BORDER_OFFSET, side='right') - 1
width = ends[run_idx] - starts[run_idx] + 1
confidence = np.clip(1 - width * dt, 0, 1)
return lag, corr, confidence
def retrieve_initial_lag(params: Params, CP: car.CarParams):
last_lag_data = params.get("LiveDelay")
last_carparams_data = params.get("CarParamsPrevRoute")
if last_lag_data is not None:
try:
with log.Event.from_bytes(last_lag_data) as last_lag_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
ld = last_lag_msg.lateralDelay
if last_CP.carFingerprint != CP.carFingerprint:
raise Exception("Car model mismatch")
lag, valid_blocks, status, version = ld.lateralDelayEstimate, ld.validBlocks, ld.status, ld.version
assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks"
assert status != log.LateralDelay.Status.invalid, "Lag estimate is invalid"
assert version == VERSION, f"Lag estimate is from a different version (got {version}, expected {VERSION})"
return lag, valid_blocks
except Exception as e:
cloudlog.error(f"Failed to retrieve initial lag: {e}")
params.remove("LiveDelay")
return None

336
iqpilot/selfdrive/locationd/locationd.py Executable file
View File

@@ -0,0 +1,336 @@
#!/usr/bin/env python3
import os
import time
import capnp
import numpy as np
from enum import Enum
from collections import defaultdict
from iqpilot.cereal import log, messaging
from iqpilot.cereal.services import SERVICE_LIST
from iqpilot.common.transformations.orientation import rot_from_euler
from iqpilot.common.realtime import config_realtime_process
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
from iqpilot.selfdrive.locationd.helpers import rotate_std
from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
ACCEL_SANITY_CHECK = 100.0 # m/s^2
ROTATION_SANITY_CHECK = 10.0 # rad/s
TRANS_SANITY_CHECK = 200.0 # m/s
CALIB_RPY_SANITY_CHECK = 0.5 # rad (+- 30 deg)
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_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)
measurement.xStd, measurement.yStd, measurement.zStd = map(float, stds)
measurement.valid = valid
class HandleLogResult(Enum):
SUCCESS = 0
TIMING_INVALID = 1
INPUT_INVALID = 2
SENSOR_SOURCE_INVALID = 3
class LocationEstimator:
def __init__(self, debug: bool):
self.kf = PoseKalman(MAX_FILTER_REWIND_TIME)
self.debug = debug
self.posenet_stds = np.array([POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2))
self.car_speed = 0.0
self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std
self.device_from_calib = np.eye(3)
obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION]
self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P):
self.kf.init_state(x_initial, covs=P_initial, filter_time=t)
def _validate_sensor_source(self, source: log.SensorEventData.SensorSource):
# some segments have two IMUs, ignore the second one
return source != log.SensorEventData.SensorSource.bmx055
def _validate_sensor_time(self, sensor_time: float, t: float):
# ignore empty readings
if sensor_time == 0:
return False
# sensor time and log time should be close
sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF
if sensor_time_invalid:
cloudlog.warning("Sensor reading ignored, sensor timestamp more than 100ms off from log time")
return not sensor_time_invalid
def _validate_timestamp(self, t: float):
kf_t = self.kf.t
invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
if invalid:
cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter")
return not invalid
def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray):
all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all()
if not all_finite:
cloudlog.error("Non-finite values detected, kalman reset")
self.reset(t)
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult:
new_x, new_P = None, None
if which == "accelerometer" and msg.which() == "acceleration":
sensor_time = msg.timestamp * 1e-9
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
return HandleLogResult.TIMING_INVALID
if not self._validate_sensor_source(msg.source):
return HandleLogResult.SENSOR_SOURCE_INVALID
v = msg.acceleration.v
meas = np.array([-v[2], -v[1], -v[0]])
if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas)
if acc_res is not None:
_, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res
self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err)
self.observations[ObservationKind.PHONE_ACCEL] = meas
elif which == "gyroscope" and msg.which() == "gyroUncalibrated":
sensor_time = msg.timestamp * 1e-9
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
return HandleLogResult.TIMING_INVALID
if not self._validate_sensor_source(msg.source):
return HandleLogResult.SENSOR_SOURCE_INVALID
v = msg.gyroUncalibrated.v
meas = np.array([-v[2], -v[1], -v[0]])
gyro_bias = self.kf.x[States.GYRO_BIAS]
gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0])
gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1]
gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold
if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid:
return HandleLogResult.INPUT_INVALID
gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas)
if gyro_res is not None:
_, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res
self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err)
self.observations[ObservationKind.PHONE_GYRO] = meas
elif which == "carState":
self.car_speed = abs(msg.vEgo)
elif which == "extrinsicsCalibration":
# Note that we use this message during calibration
calib = get_calibrated_rpy(msg)
if calib is None and len(msg.rpyCalib) > 0:
calib = np.array(msg.rpyCalib)
if calib is not None:
if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
self.device_from_calib = rot_from_euler(calib)
elif which == "cameraOdometry":
if not self._validate_timestamp(t):
return HandleLogResult.TIMING_INVALID
rot_device = np.matmul(self.device_from_calib, np.array(msg.rot))
trans_device = np.matmul(self.device_from_calib, np.array(msg.trans))
if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
rot_calib_std = np.array(msg.rotStd)
trans_calib_std = np.array(msg.transStd)
if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
self.posenet_stds = np.roll(self.posenet_stds, -1)
self.posenet_stds[-1] = trans_calib_std[0]
# Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
rot_calib_std *= 10
trans_calib_std *= 2
rot_device_std = rotate_std(self.device_from_calib, rot_calib_std)
trans_device_std = rotate_std(self.device_from_calib, trans_calib_std)
rot_device_noise = rot_device_std ** 2
trans_device_noise = trans_device_std ** 2
cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)]))
cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)]))
self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]])
if cam_odo_rot_res is not None:
_, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res
self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err)
self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device
if cam_odo_trans_res is not None:
_, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res
self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err)
self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device
if new_x is not None and new_P is not None:
self._finite_check(t, new_x, new_P)
return HandleLogResult.SUCCESS
def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool):
state, cov = self.kf.x, self.kf.P
std = np.sqrt(np.diag(cov))
orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION]
velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY]
angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY]
acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION]
msg = messaging.new_message("deviceMotion")
msg.valid = filter_valid
deviceMotion = msg.deviceMotion
init_xyz_measurement(deviceMotion.orientationNED, orientation_ned, orientation_ned_std, filter_valid)
init_xyz_measurement(deviceMotion.velocityDevice, velocity_device, velocity_device_std, filter_valid)
init_xyz_measurement(deviceMotion.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid)
init_xyz_measurement(deviceMotion.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid)
if self.debug:
deviceMotion.debugFilterState.value = state.tolist()
deviceMotion.debugFilterState.std = std.tolist()
deviceMotion.debugFilterState.valid = filter_valid
deviceMotion.debugFilterState.observations = [
{'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()}
for k in self.observations.keys()
]
old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF])
new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:])
std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0
deviceMotion.inputsOK = inputs_valid
deviceMotion.posenetOK = not std_spike or self.car_speed <= 5.0
deviceMotion.sensorsOK = sensors_valid
return msg
def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation):
cur_time = time.monotonic()
for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]:
if len(msgs) > 0:
sensor_valid[which] = msgs[-1].valid
sensor_recv_time[which] = cur_time
if not simulation:
sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1
else:
sensor_alive[which] = len(msgs) > 0
return all(sensor_alive.values()) and all(sensor_valid.values())
def main():
config_realtime_process([0, 1, 2, 3], 5)
DEBUG = bool(int(os.getenv("DEBUG", "0")))
SIMULATION = bool(int(os.getenv("SIMULATION", "0")))
pm = messaging.PubMaster(['deviceMotion'])
sm = messaging.SubMaster(['carState', 'extrinsicsCalibration', 'cameraOdometry'], poll='cameraOdometry')
# separate sensor sockets for efficiency
sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']]
sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float)
params = Params()
estimator = LocationEstimator(DEBUG)
filter_initialized = False
critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
observation_input_invalid = defaultdict(int)
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_data = params.get("LocationFilterInitialState")
if initial_pose_data is not None:
with log.Event.from_bytes(initial_pose_data) as lp_msg:
filter_state = lp_msg.deviceMotion.debugFilterState
x_initial = np.array(filter_state.value, dtype=np.float64) if len(filter_state.value) != 0 else PoseKalman.initial_x
P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P
estimator.reset(None, x_initial, P_initial)
while True:
sm.update()
acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets)
if filter_initialized:
msgs = []
for msg in acc_msgs + gyro_msgs:
t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which())
msgs.append((t, valid, which, data))
for which, updated in sm.updated.items():
if not updated:
continue
t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which]
msgs.append((t, valid, which, data))
for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]):
if valid:
t = log_mono_time * 1e-9
res = estimator.handle_log(t, which, msg)
if which not in critcal_services:
continue
if res == HandleLogResult.TIMING_INVALID:
cloudlog.warning(f"Observation {which} ignored due to failed timing check")
observation_input_invalid[which] += 1
elif res == HandleLogResult.INPUT_INVALID:
cloudlog.warning(f"Observation {which} ignored due to failed sanity check")
observation_input_invalid[which] += 1
elif res == HandleLogResult.SUCCESS:
observation_input_invalid[which] *= input_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[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)
pm.send("deviceMotion", msg)
if __name__ == "__main__":
main()

1
iqpilot/selfdrive/locationd/models/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
generated/

0
iqpilot/selfdrive/locationd/models/__init__.py Normal file
View File

94
iqpilot/selfdrive/locationd/models/car_kf.py Normal file
View File

@@ -0,0 +1,94 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
from typing import Any
import numpy as np
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import car_predict, car_update
except ModuleNotFoundError:
car_predict = None
car_update = None
class States:
STIFFNESS = slice(0, 1)
STEER_RATIO = slice(1, 2)
ANGLE_OFFSET = slice(2, 3)
ANGLE_OFFSET_FAST = slice(3, 4)
VELOCITY = slice(4, 6)
YAW_RATE = slice(6, 7)
STEER_ANGLE = slice(7, 8)
ROAD_ROLL = slice(8, 9)
def _transition(state: np.ndarray, dt: float, values: dict[str, float]) -> np.ndarray:
result = state.copy()
stiffness = state[0]
steer_ratio = state[1]
angle = state[7] - state[2] - state[3]
speed, lateral_speed = state[4:6]
yaw_rate = state[6]
mass = values["mass"]
inertia = values["rotational_inertia"]
front = values["center_to_front"]
rear = values["center_to_rear"]
front_stiffness = stiffness * values["stiffness_front"]
rear_stiffness = stiffness * values["stiffness_rear"]
lateral_dot = -(front_stiffness + rear_stiffness) * lateral_speed / (mass * speed)
lateral_dot += (-(front_stiffness * front - rear_stiffness * rear) / (mass * speed) - speed) * yaw_rate
lateral_dot += front_stiffness * angle / (mass * steer_ratio) - ACCELERATION_DUE_TO_GRAVITY * state[8]
yaw_dot = -(front_stiffness * front - rear_stiffness * rear) * lateral_speed / (inertia * speed)
yaw_dot -= (front_stiffness * front**2 + rear_stiffness * rear**2) * yaw_rate / (inertia * speed)
yaw_dot += front_stiffness * front * angle / (inertia * steer_ratio)
result[5] += dt * lateral_dot
result[6] += dt * yaw_dot
return result
class CarKalman(EstimatorModel):
name = "car"
initial_x = np.array([1.0, 15.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0])
Q = np.diag([(.05 / 100)**2, .01**2, math.radians(0.02)**2, math.radians(0.25)**2,
.1**2, .01**2, math.radians(0.1)**2, math.radians(0.1)**2, math.radians(1)**2])
P_initial = Q.copy()
obs_noise: dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
def __init__(self):
self.native_parameters = np.zeros(6)
measurements = {
ObservationKind.ROAD_FRAME_YAW_RATE: lambda state, _: state[6:7],
ObservationKind.ROAD_FRAME_XY_SPEED: lambda state, _: state[4:6],
ObservationKind.ROAD_FRAME_X_SPEED: lambda state, _: state[4:5],
ObservationKind.STEER_ANGLE: lambda state, _: state[7:8],
ObservationKind.ANGLE_OFFSET_FAST: lambda state, _: state[3:4],
ObservationKind.STEER_RATIO: lambda state, _: state[1:2],
ObservationKind.STIFFNESS: lambda state, _: state[0:1],
ObservationKind.ROAD_ROLL: lambda state, _: state[8:9],
}
def native_predict(state, covariance, dt, process_noise, _):
car_predict(state, covariance, process_noise, dt, self.native_parameters)
model = ModelDefinition(9, 9, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if car_predict is not None else None, native_update=car_update)
super().__init__(StateEstimator(model, self.initial_x, self.P_initial, max_rewind_age=0.8))
def set_globals(self, mass: float, rotational_inertia: float, center_to_front: float, center_to_rear: float,
stiffness_front: float, stiffness_rear: float) -> None:
self.native_parameters[:] = mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear
for name, value in locals().copy().items():
if name not in {"self"}:
self.filter.set_global(name, value)

88
iqpilot/selfdrive/locationd/models/constants.py Normal file
View File

@@ -0,0 +1,88 @@
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1
GPS_NED = 2
ODOMETRIC_SPEED = 3
PHONE_GYRO = 4
GPS_VEL = 5
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE_GPS = 7
SPEED = 8
NO_ROT = 9
PHONE_ACCEL = 10
ORB_POINT = 11
ECEF_POS = 12
CAMERA_ODO_TRANSLATION = 13
CAMERA_ODO_ROTATION = 14
ORB_FEATURES = 15
MSCKF_TEST = 16
FEATURE_TRACK_TEST = 17
LANE_PT = 18
IMU_FRAME = 19
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
ECEF_VEL = 35
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
ORB_FEATURES_WIDE = 34
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
STEER_ANGLE = 26 # [rad]
ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-]
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
ROAD_ROLL = 31 # [rad]
names = [
'Unknown',
'No observation',
'GPS NED',
'Odometric speed',
'Phone gyro',
'GPS velocity',
'GPS pseudorange',
'GPS pseudorange rate',
'Speed',
'No rotation',
'Phone acceleration',
'ORB point',
'ECEF pos',
'camera odometric translation',
'camera odometric rotation',
'ORB features',
'MSCKF test',
'Feature track test',
'Lane ecef point',
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate',
'pseudorange',
'pseudorange rate',
'Road Frame x,y speed',
'Road Frame yaw rate',
'Steer Angle',
'Fast Angle Offset',
'Stiffness',
'Steer Ratio',
'Road Frame x speed',
'Road Roll',
'ECEF orientation from GPS',
'NO accel',
'ORB features wide camera',
'ECEF_VEL',
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
ObservationKind.PSEUDORANGE_RATE_GPS,
ObservationKind.PSEUDORANGE_GLONASS,
ObservationKind.PSEUDORANGE_RATE_GLONASS]

67
iqpilot/selfdrive/locationd/models/pose_kf.py Normal file
View File

@@ -0,0 +1,67 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import numpy as np
from iqpilot.common.transformations.orientation import euler_from_rot, rot_from_euler
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import pose_predict, pose_update
except ModuleNotFoundError:
pose_predict = None
pose_update = None
EARTH_G = 9.81
class States:
NED_ORIENTATION = slice(0, 3)
DEVICE_VELOCITY = slice(3, 6)
ANGULAR_VELOCITY = slice(6, 9)
GYRO_BIAS = slice(9, 12)
ACCELERATION = slice(12, 15)
ACCEL_BIAS = slice(15, 18)
def _transition(state: np.ndarray, dt: float, _: dict[str, float]) -> np.ndarray:
result = state.copy()
result[States.DEVICE_VELOCITY] += dt * state[States.ACCELERATION]
rotation = rot_from_euler(state[States.NED_ORIENTATION]) @ rot_from_euler(dt * state[States.ANGULAR_VELOCITY])
result[States.NED_ORIENTATION] = euler_from_rot(rotation)
return result
def _phone_acceleration(state: np.ndarray, _: dict[str, float]) -> np.ndarray:
device_from_ned = rot_from_euler(state[States.NED_ORIENTATION]).T
centripetal = np.cross(state[States.ANGULAR_VELOCITY], state[States.DEVICE_VELOCITY])
return device_from_ned @ np.array([0.0, 0.0, -EARTH_G]) + state[States.ACCELERATION] + centripetal + state[States.ACCEL_BIAS]
class PoseKalman(EstimatorModel):
name = "pose"
initial_x = np.zeros(18)
initial_P = np.diag([0.01**2] * 3 + [10**2] * 3 + [1**2] * 6 + [100**2] * 3 + [0.01**2] * 3)
Q = np.diag([0.001**2] * 3 + [0.01**2] * 3 + [0.1**2] * 3 + [(0.005 / 100)**2] * 3 + [3**2] * 3 + [0.005**2] * 3)
obs_noise = {
ObservationKind.PHONE_GYRO: np.diag([0.025**2] * 3),
ObservationKind.PHONE_ACCEL: np.diag([0.5**2] * 3),
ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2] * 3),
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2] * 3),
}
def __init__(self, max_rewind_age: float):
measurements = {
ObservationKind.PHONE_GYRO: lambda state, _: state[States.ANGULAR_VELOCITY] + state[States.GYRO_BIAS],
ObservationKind.PHONE_ACCEL: _phone_acceleration,
ObservationKind.CAMERA_ODO_TRANSLATION: lambda state, _: state[States.DEVICE_VELOCITY],
ObservationKind.CAMERA_ODO_ROTATION: lambda state, _: state[States.ANGULAR_VELOCITY],
}
def native_predict(state, covariance, dt, process_noise, _):
pose_predict(state, covariance, process_noise, dt)
model = ModelDefinition(18, 18, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if pose_predict is not None else None, native_update=pose_update)
super().__init__(StateEstimator(model, self.initial_x, self.initial_P, max_rewind_age=max_rewind_age))

267
iqpilot/selfdrive/locationd/paramsd.py Executable file
View File

@@ -0,0 +1,267 @@
#!/usr/bin/env python3
import numpy as np
import capnp
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import car, log
from iqpilot.common.issue_debug import log_issue_limited
from iqpilot.common.params import Params
from iqpilot.common.realtime import DT_MDL
from iqpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
from iqpilot.common.swaglog import cloudlog
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s
ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
ROLL_LOWERED_MAX = np.radians(8)
ROLL_STD_MAX = np.radians(1.5)
LATERAL_ACC_SENSOR_THRESHOLD = 4.0
OFFSET_MAX = 16.0 # angle
OFFSET_LOWERED_MAX = 14.0 # angle
MIN_ACTIVE_SPEED = 1.0
LOW_ACTIVE_SPEED = 10.0
class VehicleParamsEstimator:
def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
self.kf = CarKalman()
self.x_initial = CarKalman.initial_x.copy()
self.x_initial[States.STEER_RATIO] = steer_ratio
self.x_initial[States.STIFFNESS] = stiffness_factor
self.x_initial[States.ANGLE_OFFSET] = angle_offset
self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial
self.kf.set_globals(
mass=CP.mass,
rotational_inertia=CP.rotationalInertia,
center_to_front=CP.centerToFront,
center_to_rear=CP.wheelbase - CP.centerToFront,
stiffness_front=CP.tireStiffnessFront,
stiffness_rear=CP.tireStiffnessRear
)
self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
self.calibrator = PoseCalibrator()
self.observed_speed = 0.0
self.observed_yaw_rate = 0.0
self.observed_roll = 0.0
self.avg_offset_valid = True
self.total_offset_valid = True
self.roll_valid = True
self.reset(None)
def reset(self, t: float | None):
self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t)
self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False
self.avg_angle_offset = self.angle_offset
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
if which == 'deviceMotion':
device_pose = Pose.from_live_pose(msg)
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
yaw_rate_valid = msg.angularVelocityDevice.valid
yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s
yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s
if not yaw_rate_valid:
# This is done to bound the yaw rate estimate when localizer values are invalid or calibrating
yaw_rate, yaw_rate_std = 0.0, np.radians(10.0)
self.observed_yaw_rate = yaw_rate
localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
if roll_valid:
roll = localizer_roll
# Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar?
roll_std = 2 * localizer_roll_std
else:
# This is done to bound the road roll estimate when localizer values are invalid
roll = 0.0
roll_std = np.radians(10.0)
self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA)
if self.active:
if msg.posenetOK:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-self.observed_yaw_rate]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ROAD_ROLL,
np.array([[self.observed_roll]]),
np.array([np.atleast_2d(roll_std**2)]))
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
# We observe the current stiffness and steer ratio (with a high observation noise) to bound
# the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the
# states in longer routes (especially straight stretches).
stiffness = float(self.kf.x[States.STIFFNESS].item())
steer_ratio = float(self.kf.x[States.STEER_RATIO].item())
self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
elif which == 'extrinsicsCalibration':
self.calibrator.feed_live_calib(msg)
elif which == 'carState':
steering_angle = msg.steeringAngleDeg
in_linear_region = abs(steering_angle) < 45
self.observed_speed = msg.vEgo
self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region
if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]]))
if not self.active:
# Reset time when stopped so uncertainty doesn't grow
self.kf.filter.set_filter_time(t)
self.kf.filter.reset_rewind()
def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
x = self.kf.x
P = np.sqrt(self.kf.P.diagonal())
if not np.all(np.isfinite(x)):
cloudlog.error("NaN in vehicleParameters estimate. Resetting to default values")
self.reset(self.kf.t)
x = self.kf.x
self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()),
self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA)
self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA)
self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
roll_std = float(P[States.ROAD_ROLL].item())
if self.active and self.observed_speed > LOW_ACTIVE_SPEED:
# Account for the opposite signs of the yaw rates
# At low speeds, bumping into a curb can cause the yaw rate to be very high
sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
else:
sensors_valid = True
self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX)
if not self.total_offset_valid:
log_issue_limited(
"paramsd_angle_offset_invalid",
"calibration",
f"paramsd angle offset invalid angleOffsetDeg={self.angle_offset:.2f} "
f"angleOffsetAverageDeg={self.avg_angle_offset:.2f} speed={self.observed_speed:.2f}",
interval_sec=2.0,
)
msg = messaging.new_message('vehicleParameters')
msg.valid = valid
vehicleParameters = msg.vehicleParameters
vehicleParameters.posenetValid = True
vehicleParameters.sensorValid = sensors_valid
vehicleParameters.steerRatio = float(x[States.STEER_RATIO].item())
vehicleParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
vehicleParameters.roll = float(self.roll)
vehicleParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
vehicleParameters.angleOffsetDeg = float(self.angle_offset)
vehicleParameters.steerRatioValid = self.min_sr <= vehicleParameters.steerRatio <= self.max_sr
vehicleParameters.stiffnessFactorValid = 0.2 <= vehicleParameters.stiffnessFactor <= 5.0
vehicleParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
vehicleParameters.angleOffsetValid = bool(self.total_offset_valid)
vehicleParameters.valid = all((
vehicleParameters.angleOffsetAverageValid,
vehicleParameters.angleOffsetValid ,
self.roll_valid,
roll_std < ROLL_STD_MAX,
vehicleParameters.stiffnessFactorValid,
vehicleParameters.steerRatioValid,
))
vehicleParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
vehicleParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
vehicleParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
vehicleParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
if debug:
vehicleParameters.debugFilterState = log.VehicleParameters.FilterState.new_message()
vehicleParameters.debugFilterState.value = x.tolist()
vehicleParameters.debugFilterState.std = P.tolist()
return msg
def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float):
if current_valid:
current_valid = abs(val) < threshold
else:
current_valid = abs(val) < lowered_threshold
return current_valid
# TODO: Remove this function after few releases (added in 0.9.9)
def migrate_cached_vehicle_params_if_needed(params: Params):
last_parameters_data_old = params.get("LiveParameters")
last_parameters_data = params.get("LiveParametersV2")
if last_parameters_data_old is None or last_parameters_data is not None:
return
try:
last_parameters_msg = messaging.new_message('vehicleParameters')
last_parameters_msg.vehicleParameters.valid = True
last_parameters_msg.vehicleParameters.steerRatio = last_parameters_data_old['steerRatio']
last_parameters_msg.vehicleParameters.stiffnessFactor = last_parameters_data_old['stiffnessFactor']
last_parameters_msg.vehicleParameters.angleOffsetAverageDeg = last_parameters_data_old['angleOffsetAverageDeg']
params.put("LiveParametersV2", last_parameters_msg.to_bytes())
except Exception as e:
cloudlog.error(f"Failed to perform parameter migration: {e}")
params.remove("LiveParameters")
def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: bool, debug: bool):
last_parameters_data = params.get("LiveParametersV2")
last_carparams_data = params.get("CarParamsPrevRoute")
steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None
retrieve_success = False
if last_parameters_data is not None and last_carparams_data is not None:
try:
with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
lp = last_lp_msg.vehicleParameters
# Check if car model matches
if last_CP.carFingerprint != CP.carFingerprint:
raise Exception("Car model mismatch")
# Check if starting values are sane
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
steer_ratio_sane = min_sr <= lp.steerRatio <= max_sr
if not steer_ratio_sane:
raise Exception(f"Invalid starting values found {lp}")
initial_filter_std = np.array(lp.debugFilterState.std)
if debug and len(initial_filter_std) != 0:
p_initial = np.diag(initial_filter_std)
steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
retrieve_success = True
except Exception as e:
cloudlog.error(f"Failed to retrieve initial values: {e}")
params.remove("LiveParametersV2")
if not replay:
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
# Without a way to detect this we have to reset the stiffness every drive
stiffness_factor = 1.0
if not retrieve_success:
cloudlog.info("Vehicle parameter estimator resetting to default values")
return steer_ratio, stiffness_factor, angle_offset_deg, p_initial

1
iqpilot/selfdrive/locationd/test/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
out/

0
iqpilot/selfdrive/locationd/test/__init__.py Normal file
View File

61
iqpilot/selfdrive/locationd/test/test_calibration_helpers.py Normal file
View File

@@ -0,0 +1,61 @@
import numpy as np
import pytest
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import log
from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy, get_render_path_height
from iqpilot.selfdrive.locationd.helpers import PoseCalibrator
def build_live_calibration(status: log.ExtrinsicsCalibration.Status,
rpy: tuple[float, float, float] = (0.0, 0.0, 0.0),
height: tuple[float, ...] = (1.22,)):
msg = messaging.new_message("extrinsicsCalibration")
msg.extrinsicsCalibration.calStatus = status
msg.extrinsicsCalibration.rpyCalib = list(rpy)
msg.extrinsicsCalibration.height = list(height)
return msg.extrinsicsCalibration
def test_get_calibrated_rpy_requires_calibrated_status():
live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.uncalibrated, rpy=(0.1, 0.2, 0.3))
assert get_calibrated_rpy(live_calib) is None
live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.calibrated, rpy=(0.1, 0.2, 0.3))
np.testing.assert_allclose(get_calibrated_rpy(live_calib), np.array([0.1, 0.2, 0.3], dtype=np.float32))
def test_get_render_path_height_uses_default_until_calibrated():
live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.uncalibrated, height=(1.5,))
assert get_render_path_height(live_calib) == float(HEIGHT_INIT[0])
live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.calibrated, height=(1.5,))
assert get_render_path_height(live_calib) == 1.5
def test_calibrated_values_are_used_even_when_identity_override_is_disabled():
live_calib = build_live_calibration(log.ExtrinsicsCalibration.Status.calibrated, rpy=(0.3, -0.2, 0.1), height=(1.6,))
np.testing.assert_allclose(get_calibrated_rpy(live_calib), np.array([0.3, -0.2, 0.1], dtype=np.float32))
assert get_render_path_height(live_calib) == pytest.approx(1.6)
def test_pose_calibrator_holds_identity_until_calibrated():
calibrator = PoseCalibrator()
uncalibrated = build_live_calibration(log.ExtrinsicsCalibration.Status.uncalibrated, rpy=(0.1, 0.2, 0.3))
calibrator.feed_live_calib(uncalibrated)
np.testing.assert_allclose(calibrator.calib_from_device, np.eye(3))
assert not calibrator.calib_valid
calibrated = build_live_calibration(log.ExtrinsicsCalibration.Status.calibrated, rpy=(0.0, 0.1, 0.0))
calibrator.feed_live_calib(calibrated)
assert not np.allclose(calibrator.calib_from_device, np.eye(3))
assert calibrator.calib_valid
recalibrating = build_live_calibration(log.ExtrinsicsCalibration.Status.recalibrating, rpy=(0.2, 0.2, 0.2))
frozen_transform = calibrator.calib_from_device.copy()
calibrator.feed_live_calib(recalibrating)
np.testing.assert_allclose(calibrator.calib_from_device, frozen_transform)
assert not calibrator.calib_valid

117
iqpilot/selfdrive/locationd/test/test_calibrationd.py Normal file
View File

@@ -0,0 +1,117 @@
import random
import numpy as np
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import log
from iqpilot.common.params import Params
from iqpilot.selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, \
MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT, MAX_ALLOWED_PITCH_SPREAD, MAX_ALLOWED_YAW_SPREAD
def process_messages(c, cam_odo_calib, cycles,
cam_odo_speed=MIN_SPEED_FILTER + 1,
carstate_speed=MIN_SPEED_FILTER + 1,
cam_odo_yr=0.0,
cam_odo_speed_std=1e-3,
cam_odo_height_std=1e-3):
old_rpy_weight_prev = 0.0
for _ in range(cycles):
assert (old_rpy_weight_prev - c.old_rpy_weight < 1/SMOOTH_CYCLES + 1e-3)
old_rpy_weight_prev = c.old_rpy_weight
c.handle_v_ego(carstate_speed)
c.handle_cam_odom([cam_odo_speed,
np.sin(cam_odo_calib[2]) * cam_odo_speed,
-np.sin(cam_odo_calib[1]) * cam_odo_speed],
[0.0, 0.0, cam_odo_yr],
[0.0, 0.0, 0.0],
[cam_odo_speed_std, cam_odo_speed_std, cam_odo_speed_std],
[0.0, 0.0, HEIGHT_INIT.item()],
[cam_odo_height_std, cam_odo_height_std, cam_odo_height_std])
class TestCalibrationd:
def test_read_saved_params(self):
msg = messaging.new_message('extrinsicsCalibration')
msg.extrinsicsCalibration.validBlocks = random.randint(1, 10)
msg.extrinsicsCalibration.rpyCalib = [0.0, 0.01, 0.01]
msg.extrinsicsCalibration.height = HEIGHT_INIT.tolist()
Params().put("CalibrationParams", msg.to_bytes())
c = Calibrator(param_put=True)
np.testing.assert_allclose(msg.extrinsicsCalibration.rpyCalib, c.rpy)
np.testing.assert_allclose(msg.extrinsicsCalibration.height, c.height)
assert msg.extrinsicsCalibration.validBlocks == c.valid_blocks
def test_calibration_basics(self):
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED)
assert c.valid_blocks == INPUTS_WANTED
np.testing.assert_allclose(c.rpy, np.zeros(3))
np.testing.assert_allclose(c.height, HEIGHT_INIT)
c.reset()
def test_calibration_low_speed_reject(self):
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, carstate_speed=MIN_SPEED_FILTER - 1)
assert c.valid_blocks == 0
np.testing.assert_allclose(c.rpy, np.zeros(3))
np.testing.assert_allclose(c.height, HEIGHT_INIT)
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED,
cam_odo_speed=MIN_SPEED_FILTER - 10, carstate_speed=MIN_SPEED_FILTER + 5)
assert c.valid_blocks == INPUTS_WANTED
np.testing.assert_allclose(c.rpy[1:], np.zeros(2), atol=1e-6)
np.testing.assert_allclose(c.height, HEIGHT_INIT)
def test_calibration_yaw_rate_reject(self):
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_yr=MAX_YAW_RATE_FILTER)
assert c.valid_blocks == 0
np.testing.assert_allclose(c.rpy, np.zeros(3))
np.testing.assert_allclose(c.height, HEIGHT_INIT)
def test_calibration_speed_std_reject(self):
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed_std=1e3)
assert c.valid_blocks == 0
np.testing.assert_allclose(c.rpy, np.zeros(3))
def test_calibration_speed_std_height_reject(self):
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_height_std=1e3)
assert c.valid_blocks == INPUTS_WANTED
np.testing.assert_allclose(c.rpy, np.zeros(3))
def test_calibration_auto_reset(self):
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
assert c.valid_blocks == INPUTS_NEEDED
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3)
process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10)
assert c.valid_blocks == INPUTS_NEEDED + 1
assert c.cal_status == log.ExtrinsicsCalibration.Status.calibrated
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
assert c.valid_blocks == INPUTS_NEEDED
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10)
assert c.valid_blocks == 1
assert c.cal_status == log.ExtrinsicsCalibration.Status.recalibrating
np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2)
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
assert c.valid_blocks == INPUTS_NEEDED
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10)
assert c.valid_blocks == 1
assert c.cal_status == log.ExtrinsicsCalibration.Status.recalibrating
np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2)

43
iqpilot/selfdrive/locationd/test/test_helpers.py Normal file
View File

@@ -0,0 +1,43 @@
import numpy as np
import pytest
from iqpilot.selfdrive.locationd.helpers import ParameterEstimator, PointBuckets, fft_next_good_size
class ScalarBuckets(PointBuckets):
def add_point(self, x, y):
for bounds, bucket in self.buckets.items():
if bounds[0] <= x < bounds[1]:
bucket.append([x, y])
return
raise ValueError(x)
def test_fft_next_good_size_small_input():
assert fft_next_good_size(6) == 6
def test_point_buckets_base_requires_an_insertion_policy():
buckets = PointBuckets([(0, 1)], [1], 1, 2, 2)
with pytest.raises(NotImplementedError):
buckets.add_point(0.5, 1.0)
def test_point_buckets_load_and_retrieve():
buckets = ScalarBuckets([(0, 1), (1, 2)], [1, 1], 2, 3, 2)
points = [[0.25, 10.0], [1.25, 20.0], [0.75, 30.0]]
buckets.load_points(points)
assert buckets.is_valid()
assert buckets.is_calculable()
np.testing.assert_allclose(buckets.get_points(), [[0.25, 10.0], [0.75, 30.0], [1.25, 20.0]])
assert buckets.get_points(2).shape == (2, 2)
@pytest.mark.parametrize("method,args", [
("reset", ()),
("handle_log", (0, "carState", None)),
("get_msg", (True, False)),
])
def test_parameter_estimator_requires_an_implementation(method, args):
with pytest.raises(NotImplementedError):
getattr(ParameterEstimator(), method)(*args)

136
iqpilot/selfdrive/locationd/test/test_lagd.py Normal file
View File

@@ -0,0 +1,136 @@
import random
import numpy as np
import time
import pytest
from iqpilot.cereal import messaging, log, car
from iqpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \
BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC, VERSION, MIN_LAG, MAX_LAG
from iqpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams
from iqpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE
from iqpilot.common.params import Params
from iqpilot.tools.lib.logreader import LogReader
MAX_ERR_FRAMES = 1
DT = 0.05
LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES = int(round(MIN_LAG / DT)), int(round(MAX_LAG / DT))
def process_messages(estimator, lag_frames, n_frames, vego=25.0, rejection_threshold=0.0):
for i in range(n_frames):
t = i * estimator.dt
desired_la = np.cos(10 * t) * 0.3
actual_la = np.cos(10 * (t - lag_frames * estimator.dt)) * 0.3
# if sample is masked out, set it to desired value (no lag)
rejected = random.uniform(0, 1) < rejection_threshold
if rejected:
actual_la = desired_la
desired_cuvature = float(desired_la / (vego ** 2))
actual_yr = float(actual_la / vego)
msgs = [
(t, "carControl", car.CarControl(latActive=not rejected)),
(t, "carState", car.CarState(vEgo=vego, steeringPressed=False)),
(t, "controlsState", log.ControlsState(desiredCurvature=desired_cuvature)),
(t, "deviceMotion", log.DeviceMotion(angularVelocityDevice=log.DeviceMotion.XYZMeasurement(z=actual_yr, valid=True),
posenetOK=True, inputsOK=True)),
(t, "extrinsicsCalibration", log.ExtrinsicsCalibration(rpyCalib=[0, 0, 0], calStatus=log.ExtrinsicsCalibration.Status.calibrated)),
]
for t, w, m in msgs:
estimator.handle_log(t, w, m)
estimator.update_points()
estimator.update_estimate()
class TestLagd:
def test_read_saved_params(self):
params = Params()
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
CP = next(m for m in lr if m.which() == "carParams").carParams
msg = messaging.new_message('lateralDelay')
msg.lateralDelay.lateralDelayEstimate = random.random()
msg.lateralDelay.validBlocks = random.randint(1, 10)
msg.lateralDelay.version = VERSION
params.put("LiveDelay", msg.to_bytes())
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
saved_lag_params = retrieve_initial_lag(params, CP)
assert saved_lag_params is not None
lag, valid_blocks = saved_lag_params
assert lag == msg.lateralDelay.lateralDelayEstimate
assert valid_blocks == msg.lateralDelay.validBlocks
def test_ncc(self):
lag_frames = random.randint(1, 19)
desired_sig = np.sin(np.arange(0.0, 10.0, 0.1))
actual_sig = np.sin(np.arange(0.0, 10.0, 0.1) - lag_frames * 0.1)
mask = np.ones(len(desired_sig), dtype=bool)
corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
assert np.argmax(corr) == lag_frames
# add some noise
desired_sig += np.random.normal(0, 0.05, len(desired_sig))
actual_sig += np.random.normal(0, 0.05, len(actual_sig))
corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1)
# mask out 40% of the values, and make them noise
mask = np.random.choice([True, False], size=len(desired_sig), p=[0.6, 0.4])
desired_sig[~mask] = np.random.normal(0, 1, size=np.sum(~mask))
actual_sig[~mask] = np.random.normal(0, 1, size=np.sum(~mask))
corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20]
assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1)
def test_empty_estimator(self):
mocked_CP = car.CarParams(steerActuatorDelay=0.5)
estimator = LateralLagEstimator(mocked_CP, DT)
msg = estimator.get_msg(True)
assert msg.lateralDelay.status == 'unestimated'
assert np.allclose(msg.lateralDelay.lateralDelay, estimator.initial_lag)
assert np.allclose(msg.lateralDelay.lateralDelayEstimate, estimator.initial_lag)
assert msg.lateralDelay.validBlocks == 0
assert msg.lateralDelay.calPerc == 0
def test_estimator_basics(self, subtests):
for lag_frames in range(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1):
with subtests.test(msg=f"lag_frames={lag_frames}"):
mocked_CP = car.CarParams(steerActuatorDelay=0.5)
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0)
process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE)
msg = estimator.get_msg(True)
assert msg.lateralDelay.status == 'estimated'
assert np.allclose(msg.lateralDelay.lateralDelay, lag_frames * DT, atol=0.01)
assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
assert msg.lateralDelay.validBlocks == BLOCK_NUM_NEEDED
assert msg.lateralDelay.calPerc == 100
def test_estimator_masking(self):
mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.5), random.randint(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1)
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1)
process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4)
msg = estimator.get_msg(True)
assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
assert msg.lateralDelay.calPerc == 100
@pytest.mark.timeout(60)
def test_estimator_performance(self):
mocked_CP = car.CarParams(steerActuatorDelay=0.5)
estimator = LateralLagEstimator(mocked_CP, DT)
ds = []
for _ in range(1000):
st = time.perf_counter()
estimator.update_points()
estimator.update_estimate()
d = time.perf_counter() - st
ds.append(d)
assert np.mean(ds) < DT

193
iqpilot/selfdrive/locationd/test/test_locationd_scenarios.py Normal file
View File

@@ -0,0 +1,193 @@
import numpy as np
import pytest
from collections import defaultdict
from enum import Enum
from iqpilot.tools.lib.logreader import LogReader
from iqpilot.selfdrive.test.process_replay.migration import migrate_all
from iqpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name
# TODO find a new segment to test
TEST_ROUTE = "4019fff6e54cf1c7|00000123--4bc0d95ef6/5"
GPS_MESSAGES = ['gpsLocationExternal', 'gpsLocation']
SELECT_COMPARE_FIELDS = {
'yaw_rate': ['angularVelocityDevice', 'z'],
'roll': ['orientationNED', 'x'],
'inputs_flag': ['inputsOK'],
'sensors_flag': ['sensorsOK'],
}
JUNK_IDX = 100
CONSISTENT_SPIKES_COUNT = 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'
def get_select_fields_data(logs):
def get_nested_keys(msg, keys):
val = None
for key in keys:
val = getattr(msg if val is None else val, key) if isinstance(key, str) else val[key]
return val
lp = [x.deviceMotion for x in logs if x.which() == 'deviceMotion']
data = defaultdict(list)
for msg in lp:
for key, fields in SELECT_COMPARE_FIELDS.items():
data[key].append(get_nested_keys(msg, fields))
for key in data:
data[key] = np.array(data[key][JUNK_IDX:], dtype=float)
return data
def modify_logs_midway(logs, which, count, fn):
non_which = [x for x in logs if x.which() != which]
which = [x for x in logs if x.which() == which]
temps = which[len(which) // 2:len(which) // 2 + count]
for i, temp in enumerate(temps):
temp = temp.as_builder()
fn(temp)
which[len(which) // 2 + i] = temp.as_reader()
return sorted(non_which + which, key=lambda x: x.logMonoTime)
def run_scenarios(scenario, logs):
if scenario == Scenario.BASE:
pass
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 or scenario == Scenario.GYRO_CONSISTENT_SPIKES:
def gyro_spike(msg):
msg.gyroscope.gyroUncalibrated.v[0] += 3.0
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 or scenario == Scenario.ACCEL_CONSISTENT_SPIKES:
def acc_spike(msg):
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):
msg.accelerometer.timestamp -= int(0.150 * 1e9)
count = 1 if scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT
logs = modify_logs_midway(logs, 'accelerometer', count, timing_spike)
replayed_logs = replay_process_with_name(name='locationd', lr=logs)
return get_select_fields_data(logs), get_select_fields_data(replayed_logs)
@pytest.mark.linux
@pytest.mark.slow
class TestLocationdScenarios:
"""
Test locationd with different scenarios. In all these scenarios, we expect the following:
- locationd kalman filter should never go unstable (we care mostly about yaw_rate, roll, gpsOK, inputsOK, sensorsOK)
- faulty values should be ignored, with appropriate flags set
"""
@classmethod
def setup_class(cls):
cls.logs = migrate_all(LogReader(TEST_ROUTE))
def test_base(self):
"""
Test: unchanged log
Expected Result:
- yaw_rate: unchanged
- roll: unchanged
"""
orig_data, replayed_data = run_scenarios(Scenario.BASE, 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))
def test_gyro_off(self):
"""
Test: no gyroscope message for the entire segment
Expected Result:
- yaw_rate: 0
- roll: 0
- sensorsOK: False
"""
_, replayed_data = run_scenarios(Scenario.GYRO_OFF, self.logs)
assert np.allclose(replayed_data['yaw_rate'], 0.0)
assert np.allclose(replayed_data['roll'], 0.0)
assert np.all(replayed_data['sensors_flag'] == 0.0)
def test_gyro_spike(self):
"""
Test: a gyroscope spike in the middle of the segment
Expected Result:
- yaw_rate: unchanged
- roll: unchanged
- inputsOK: False for some time after the spike, True for the rest
"""
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.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):
"""
Test: no accelerometer message for the entire segment
Expected Result:
- yaw_rate: 0
- roll: 0
- sensorsOK: False
"""
_, replayed_data = run_scenarios(Scenario.ACCEL_OFF, self.logs)
assert np.allclose(replayed_data['yaw_rate'], 0.0)
assert np.allclose(replayed_data['roll'], 0.0)
assert np.all(replayed_data['sensors_flag'] == 0.0)
def test_accel_spike(self):
"""
ToDo:
Test: an accelerometer spike in the middle of the segment
Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes.
"""
orig_data, replayed_data = run_scenarios(Scenario.ACCEL_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))
def test_single_timing_spike(self):
"""
Test: timing of 150ms off for the single accelerometer message in the middle of the segment
Expected Result: the message is ignored, and inputsOK is False for that time
"""
orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_SPIKE_MIDWAY, self.logs)
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_timing_spikes(self):
"""
Test: consistent timing spikes for N accelerometer messages in the middle of the segment
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'])[501] == -1.0
assert np.diff(replayed_data['inputs_flag'])[707] == 1.0

71
iqpilot/selfdrive/locationd/test/test_paramsd.py Normal file
View File

@@ -0,0 +1,71 @@
import random
import numpy as np
from iqpilot.cereal import messaging
from iqpilot.selfdrive.locationd.paramsd import retrieve_initial_vehicle_params, migrate_cached_vehicle_params_if_needed
from iqpilot.selfdrive.locationd.models.car_kf import CarKalman
from iqpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE
from iqpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams
from iqpilot.common.params import Params
from iqpilot.tools.lib.logreader import LogReader
def get_random_live_parameters(CP):
msg = messaging.new_message("vehicleParameters")
msg.vehicleParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio
msg.vehicleParameters.stiffnessFactor = random.random()
msg.vehicleParameters.angleOffsetAverageDeg = random.random()
msg.vehicleParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])]
return msg
class TestParamsd:
def test_read_saved_params(self):
params = Params()
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
CP = next(m for m in lr if m.which() == "carParams").carParams
msg = get_random_live_parameters(CP)
params.put("LiveParametersV2", msg.to_bytes())
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
migrate_cached_vehicle_params_if_needed(params) # this is not tested here but should not mess anything up or throw an error
sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
np.testing.assert_allclose(sr, msg.vehicleParameters.steerRatio)
np.testing.assert_allclose(sf, msg.vehicleParameters.stiffnessFactor)
np.testing.assert_allclose(offset, msg.vehicleParameters.angleOffsetAverageDeg)
np.testing.assert_equal(p_init.shape, CarKalman.P_initial.shape)
np.testing.assert_allclose(np.diagonal(p_init), msg.vehicleParameters.debugFilterState.std)
# TODO Remove this test after the support for old format is removed
def test_read_saved_params_old_format(self):
params = Params()
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
CP = next(m for m in lr if m.which() == "carParams").carParams
msg = get_random_live_parameters(CP)
params.put("LiveParameters", {
"steerRatio": msg.vehicleParameters.steerRatio,
"stiffnessFactor": msg.vehicleParameters.stiffnessFactor,
"angleOffsetAverageDeg": msg.vehicleParameters.angleOffsetAverageDeg,
})
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
params.remove("LiveParametersV2")
migrate_cached_vehicle_params_if_needed(params)
sr, sf, offset, _ = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
np.testing.assert_allclose(sr, msg.vehicleParameters.steerRatio)
np.testing.assert_allclose(sf, msg.vehicleParameters.stiffnessFactor)
np.testing.assert_allclose(offset, msg.vehicleParameters.angleOffsetAverageDeg)
assert params.get("LiveParametersV2") is not None
def test_read_saved_params_corrupted_old_format(self):
params = Params()
params.put("LiveParameters", {})
params.remove("LiveParametersV2")
migrate_cached_vehicle_params_if_needed(params)
assert params.get("LiveParameters") is None
assert params.get("LiveParametersV2") is None

25
iqpilot/selfdrive/locationd/test/test_torqued.py Normal file
View File

@@ -0,0 +1,25 @@
from iqpilot.cereal import car
from iqpilot.selfdrive.locationd.torqued import TorqueEstimator
def test_cal_percent():
est = TorqueEstimator(car.CarParams())
msg = est.get_msg()
assert msg.lateralTorqueParameters.calPerc == 0
for (low, high), min_pts in zip(est.filtered_points.buckets.keys(),
est.filtered_points.buckets_min_points.values(), strict=True):
for _ in range(int(min_pts)):
est.filtered_points.add_point((low + high) / 2.0, 0.0)
# enough bucket points, but not enough total points
msg = est.get_msg()
assert msg.lateralTorqueParameters.calPerc == (len(est.filtered_points) / est.min_points_total * 100 + 100) / 2
# add enough points to bucket with most capacity
key = list(est.filtered_points.buckets)[0]
for _ in range(est.min_points_total - len(est.filtered_points)):
est.filtered_points.add_point((key[0] + key[1]) / 2.0, 0.0)
msg = est.get_msg()
assert msg.lateralTorqueParameters.calPerc == 100

240
iqpilot/selfdrive/locationd/torqued.py Executable file
View File

@@ -0,0 +1,240 @@
#!/usr/bin/env python3
import numpy as np
from collections import deque, defaultdict
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import car, log
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from iqpilot.common.params import Params
from iqpilot.common.realtime import DT_MDL
from iqpilot.common.filter_simple import FirstOrderFilter
from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
HISTORY = 5 # secs
POINTS_PER_BUCKET = 1500
MIN_POINTS_TOTAL = 4000
MIN_POINTS_TOTAL_QLOG = 600
FIT_POINTS_TOTAL = 2000
FIT_POINTS_TOTAL_QLOG = 600
MIN_VEL = 15 # m/s
FRICTION_FACTOR = 1.5 # ~85% of data coverage
FACTOR_SANITY = 0.3
FACTOR_SANITY_QLOG = 0.5
FRICTION_SANITY = 0.5
FRICTION_SANITY_QLOG = 0.8
STEER_MIN_THRESHOLD = 0.02
MIN_FILTER_DECAY = 50
MAX_FILTER_DECAY = 250
LAT_ACC_THRESHOLD = 1
STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)]
MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100])
MIN_ENGAGE_BUFFER = 2 # secs
VERSION = 1 # bump this to invalidate old parameter caches
def slope2rot(slope):
sin = np.sqrt(slope ** 2 / (slope ** 2 + 1))
cos = np.sqrt(1 / (slope ** 2 + 1))
return np.array([[cos, -sin], [sin, cos]])
class TorqueBuckets(PointBuckets):
def add_point(self, x, y):
for bound_min, bound_max in self.x_bounds:
if (x >= bound_min) and (x < bound_max):
self.buckets[(bound_min, bound_max)].append([x, 1.0, y])
break
class TorqueEstimator(ParameterEstimator):
def __init__(self, CP, decimated=False, track_all_points=False):
ParameterEstimator.__init__(self)
self.CP = CP
self.hist_len = int(HISTORY / DT_MDL)
self.lag = 0.0
self.track_all_points = track_all_points # for offline analysis, without max lateral accel or max steer torque filters
if decimated:
self.min_bucket_points = MIN_BUCKET_POINTS / 10
self.min_points_total = MIN_POINTS_TOTAL_QLOG
self.fit_points = FIT_POINTS_TOTAL_QLOG
self.factor_sanity = FACTOR_SANITY_QLOG
self.friction_sanity = FRICTION_SANITY_QLOG
else:
self.min_bucket_points = MIN_BUCKET_POINTS
self.min_points_total = MIN_POINTS_TOTAL
self.fit_points = FIT_POINTS_TOTAL
self.factor_sanity = FACTOR_SANITY
self.friction_sanity = FRICTION_SANITY
self.offline_friction = 0.0
self.offline_latAccelFactor = 0.0
self.resets = 0.0
self.use_params = CP.lateralTuning.which() == 'torque'
if CP.lateralTuning.which() == 'torque':
self.offline_friction = CP.lateralTuning.torque.friction
self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor
self.calibrator = PoseCalibrator()
self.reset()
initial_params = {
'latAccelFactor': self.offline_latAccelFactor,
'latAccelOffset': 0.0,
'frictionCoefficient': self.offline_friction,
'points': []
}
self.decay = MIN_FILTER_DECAY
self.min_lataccel_factor = (1.0 - self.factor_sanity) * self.offline_latAccelFactor
self.max_lataccel_factor = (1.0 + self.factor_sanity) * self.offline_latAccelFactor
self.min_friction = (1.0 - self.friction_sanity) * self.offline_friction
self.max_friction = (1.0 + self.friction_sanity) * self.offline_friction
# try to restore cached params
params = Params()
params_cache = params.get("CarParamsPrevRoute")
torque_cache = params.get("LiveTorqueParameters")
if params_cache is not None and torque_cache is not None:
try:
with log.Event.from_bytes(torque_cache) as log_evt:
cache_ltp = log_evt.lateralTorqueParameters
with car.CarParams.from_bytes(params_cache) as msg:
cache_CP = msg
if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION):
if cache_ltp.valid:
initial_params = {
'latAccelFactor': cache_ltp.latAccelFactorFiltered,
'latAccelOffset': cache_ltp.latAccelOffsetFiltered,
'frictionCoefficient': cache_ltp.frictionCoefficientFiltered
}
initial_params['points'] = cache_ltp.points
self.decay = cache_ltp.decay
self.filtered_points.load_points(initial_params['points'])
cloudlog.info("restored torque params from cache")
except Exception:
cloudlog.exception("failed to restore cached torque params")
params.remove("LiveTorqueParameters")
self.filtered_params = {}
for param in initial_params:
self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_MDL)
@staticmethod
def get_restore_key(CP, version):
a, b = None, None
if CP.lateralTuning.which() == 'torque':
a = CP.lateralTuning.torque.friction
b = CP.lateralTuning.torque.latAccelFactor
return (CP.carFingerprint, CP.lateralTuning.which(), a, b, version)
def reset(self):
self.resets += 1.0
self.decay = MIN_FILTER_DECAY
self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len))
self.filtered_points = TorqueBuckets(x_bounds=STEER_BUCKET_BOUNDS,
min_points=self.min_bucket_points,
min_points_total=self.min_points_total,
points_per_bucket=POINTS_PER_BUCKET,
rowsize=3)
self.all_torque_points = []
def estimate_params(self):
points = self.filtered_points.get_points(self.fit_points)
# total least square solution as both x and y are noisy observations
# this is empirically the slope of the hysteresis parallelogram as opposed to the line through the diagonals
try:
_, _, v = np.linalg.svd(points, full_matrices=False)
slope, offset = -v.T[0:2, 2] / v.T[2, 2]
_, spread = np.matmul(points[:, [0, 2]], slope2rot(slope)).T
friction_coeff = np.std(spread) * FRICTION_FACTOR
except np.linalg.LinAlgError as e:
cloudlog.exception(f"Error computing live torque params: {e}")
slope = offset = friction_coeff = np.nan
return slope, offset, friction_coeff
def update_params(self, params):
self.decay = min(self.decay + DT_MDL, MAX_FILTER_DECAY)
for param, value in params.items():
self.filtered_params[param].update(value)
self.filtered_params[param].update_alpha(self.decay)
def handle_log(self, t, which, msg):
if which == "carControl":
self.raw_points["carControl_t"].append(t + self.lag)
self.raw_points["lat_active"].append(msg.latActive)
elif which == "carOutput":
self.raw_points["carOutput_t"].append(t + self.lag)
self.raw_points["steer_torque"].append(-msg.actuatorsOutput.torque)
elif which == "carState":
self.raw_points["carState_t"].append(t + self.lag)
# TODO: check if high aEgo affects resulting lateral accel
self.raw_points["vego"].append(msg.vEgo)
self.raw_points["steer_override"].append(msg.steeringPressed)
elif which == "extrinsicsCalibration":
self.calibrator.feed_live_calib(msg)
elif which == "lateralDelay":
self.lag = msg.lateralDelay
# calculate lateral accel from past steering torque
elif which == "deviceMotion":
if len(self.raw_points['steer_torque']) == self.hist_len:
device_pose = Pose.from_live_pose(msg)
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
angular_velocity_calibrated = calibrated_pose.angular_velocity
yaw_rate = angular_velocity_calibrated.yaw
roll = device_pose.orientation.roll
# check lat active up to now (without lag compensation)
lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL),
self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool)
steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL),
self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool)
vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego'])
steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']).item()
lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY).item()
if all(lat_active) and not any(steer_override) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD):
if abs(lateral_acc) <= LAT_ACC_THRESHOLD:
self.filtered_points.add_point(steer, lateral_acc)
if self.track_all_points:
self.all_torque_points.append([steer, lateral_acc])
def get_msg(self, valid=True, with_points=False):
msg = messaging.new_message('lateralTorqueParameters')
msg.valid = valid
lateralTorqueParameters = msg.lateralTorqueParameters
lateralTorqueParameters.version = VERSION
lateralTorqueParameters.useParams = self.use_params
# Calculate raw estimates when possible, only update filters when enough points are gathered
if self.filtered_points.is_calculable():
latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params()
lateralTorqueParameters.latAccelFactorRaw = float(latAccelFactor)
lateralTorqueParameters.latAccelOffsetRaw = float(latAccelOffset)
lateralTorqueParameters.frictionCoefficientRaw = float(frictionCoeff)
if self.filtered_points.is_valid():
if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]):
cloudlog.exception("Live torque parameters are invalid.")
lateralTorqueParameters.valid = False
self.reset()
else:
lateralTorqueParameters.valid = True
latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor)
frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction)
self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff})
if with_points:
lateralTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist()
lateralTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x)
lateralTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x)
lateralTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x)
lateralTorqueParameters.totalBucketPoints = len(self.filtered_points)
lateralTorqueParameters.calPerc = self.filtered_points.get_valid_percent()
lateralTorqueParameters.decay = self.decay
lateralTorqueParameters.maxResets = self.resets
return msg