sin(roll)cos(pitch) feedforward in paramsd.

params.roll is now reinterpreted as "gravity factor" rather than physical configuration, but this reinterpretation agrees perfectly with the existing linear model of acc = "roll" * g

commaai#30341

selfdrive/locationd/paramsd.py

@@ -52,8 +52,24 @@ def handle_log(self, t, which, msg):
       self.yaw_rate = msg.angularVelocityCalibrated.value[2]
       self.yaw_rate_std = msg.angularVelocityCalibrated.std[2]
 
-      localizer_roll = msg.orientationNED.value[0]
-      localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0]
+      # The "roll" param should be renamed to "gravity factor."
+      # It is the scalar projection of the ground normal onto the car's lateral axis.
+      # TODO(simontheflutist) use a dynamic model to account for speed effect.
+      # At high longitudinal speeds, the car is sort of in free fall (hence the simple calculation).
+      # But at lower speeds, there is a lot of lateral friction.
+      llk_roll = msg.orientationNED.value[0]
+      llk_roll_std = msg.orientationNED.std[0]
+      llk_pitch = msg.orientationNED.value[1]
+      llk_pitch_std = msg.orientationNED.std[1]
+      localizer_roll = math.cos(llk_pitch) * math.sin(llk_roll)
+      if np.isnan(msg.orientationNED.std[0]) or np.isnan(msg.orientationNED.std[0]):
+        localizer_roll_std = np.radians(1)
+      else:
+        # from linearization of localizer_roll as a function of roll and pitch
+        localizer_roll_std = (
+          (math.cos(llk_pitch) * math.cos(llk_roll) * llk_roll_std)**2
+          + (math.sin(llk_pitch) * math.sin(llk_roll) * llk_pitch_std)**2
+        )**0.5
       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