controlsd: use livePose (#33283)

* Pose calibrator

* Fix static analysis

* Fix static

* Fix test_latcontrol

* Fix test_latcontrol

* Update services in process replay

* Fix static

* Matmul not mul

* Add assertion

* Move pose calibration to data_sample

* Update ref commit

* Remove llk from cycle alerts

* Deprecated nogps event

* Switch power_draw to lp

* Bring back noGps alert

* Add handling code back

* get_bool

* Bring inputsok back
old-commit-hash: 9734015bbb6d448bb6b0fb453370ec702fa73106

selfdrive/locationd/helpers.py

@@ -2,6 +2,7 @@ import numpy as np
 from typing import Any
 
 from cereal import log
+from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
 
 
 def rotate_cov(rot_matrix, cov_in):
@@ -70,3 +71,69 @@ class ParameterEstimator:
 
   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.LivePose.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.LivePose) -> '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.angular_velocity)
+
+    return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib)
+
+  def feed_live_calib(self, live_calib: log.LiveCalibrationData):
+    calib_rpy = np.array(live_calib.rpyCalib)
+    device_from_calib = rot_from_euler(calib_rpy)
+    self.calib_from_device = device_from_calib.T
+    self.calib_valid = live_calib.calStatus == log.LiveCalibrationData.Status.calibrated

selfdrive/locationd/paramsd.py

@@ -9,10 +9,9 @@ from cereal import car, log
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.numpy_fast import clip
-from openpilot.common.transformations.orientation import rot_from_euler
 from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
 from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
-from openpilot.selfdrive.locationd.helpers import rotate_std
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.common.swaglog import cloudlog
 
 
@@ -40,9 +39,8 @@ class ParamsLearner:
     self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
 
     self.active = False
-    self.calibrated = False
 
-    self.calib_from_device = np.eye(3)
+    self.calibrator = PoseCalibrator()
 
     self.speed = 0.0
     self.yaw_rate = 0.0
@@ -53,15 +51,12 @@ class ParamsLearner:
 
   def handle_log(self, t, which, msg):
     if which == 'livePose':
-      angular_velocity_device = np.array([msg.angularVelocityDevice.x, msg.angularVelocityDevice.y, msg.angularVelocityDevice.z])
-      angular_velocity_device_std = np.array([msg.angularVelocityDevice.xStd, msg.angularVelocityDevice.yStd, msg.angularVelocityDevice.zStd])
-      angular_velocity_calibrated = np.matmul(self.calib_from_device, angular_velocity_device)
-      angular_velocity_calibrated_std = rotate_std(self.calib_from_device, angular_velocity_device_std)
+      device_pose = Pose.from_live_pose(msg)
+      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+      self.yaw_rate, self.yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
 
-      self.yaw_rate, self.yaw_rate_std = angular_velocity_calibrated[2], angular_velocity_calibrated_std[2]
-
-      localizer_roll = msg.orientationNED.x
-      localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.xStd) else msg.orientationNED.xStd
+      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
       self.roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
       if self.roll_valid:
         roll = localizer_roll
@@ -73,7 +68,7 @@ class ParamsLearner:
         roll_std = np.radians(10.0)
       self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
 
-      yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrated
+      yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid
       yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10  # rad/s
       yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1  # rad/s
 
@@ -101,9 +96,7 @@ class ParamsLearner:
         self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
 
     elif which == 'liveCalibration':
-      self.calibrated  = msg.calStatus == log.LiveCalibrationData.Status.calibrated
-      device_from_calib = rot_from_euler(np.array(msg.rpyCalib))
-      self.calib_from_device = device_from_calib.T
+      self.calibrator.feed_live_calib(msg)
 
     elif which == 'carState':
       self.steering_angle = msg.steeringAngleDeg

selfdrive/locationd/torqued.py

@@ -8,9 +8,8 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.swaglog import cloudlog
-from openpilot.common.transformations.orientation import rot_from_euler
 from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
-from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator
+from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -78,7 +77,7 @@ class TorqueEstimator(ParameterEstimator):
       self.offline_friction = CP.lateralTuning.torque.friction
       self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor
 
-    self.calib_from_device = np.eye(3)
+    self.calibrator = PoseCalibrator()
 
     self.reset()
 
@@ -175,17 +174,17 @@ class TorqueEstimator(ParameterEstimator):
       self.raw_points["vego"].append(msg.vEgo)
       self.raw_points["steer_override"].append(msg.steeringPressed)
     elif which == "liveCalibration":
-      device_from_calib = rot_from_euler(np.array(msg.rpyCalib))
-      self.calib_from_device = device_from_calib.T
+      self.calibrator.feed_live_calib(msg)
 
     # calculate lateral accel from past steering torque
     elif which == "livePose":
       if len(self.raw_points['steer_torque']) == self.hist_len:
-        angular_velocity_device = np.array([msg.angularVelocityDevice.x, msg.angularVelocityDevice.y, msg.angularVelocityDevice.z])
-        angular_velocity_calibrated = np.matmul(self.calib_from_device, angular_velocity_device)
+        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[2]
-        roll = msg.orientationNED.x
+        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)