  
      steering_angle = CS.steeringAngleDeg
  
      # Subtract offset. Zero angle should correspond to zero torque
      self.angle_steers_des = lat_plan.steeringAngleDeg - lat_plan.angleOffsetDeg
      steering_angle -= lat_plan.angleOffsetDeg
  
      # Update Kalman filter
      angle_steers_k = float(self.C.dot(self.x_hat))
      e = steering_angle - angle_steers_k
      self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
 
</s> remove     steering_angle = CS.steeringAngleDeg

 </s> add  </s> remove         error = self.angle_steers_des - angle_steers_k
 </s> add         error = desired_angle - angle_steers_k </s> remove     lqr_log.steeringAngleDeg = angle_steers_k + lat_plan.angleOffsetDeg
 </s> add     lqr_log.steeringAngleDeg = angle_steers_k + params.angleOffsetAverageDeg </s> remove       u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
 </s> add       u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat)) </s> remove     lqr_log.output = self.output_steer
 </s> add     lqr_log.output = output_steer
    steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg

    desired_angle = math.degrees(VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))

    instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
    desired_angle += instant_offset  # Only add offset that originates from vehicle model errors
  
      steering_angle = CS.steeringAngleDeg
  
      # Subtract offset. Zero angle should correspond to zero torque
    steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg

    desired_angle = math.degrees(VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))

    instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
    desired_angle += instant_offset  # Only add offset that originates from vehicle model errors
    steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg

    desired_angle = math.degrees(VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))

    instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
    desired_angle += instant_offset  # Only add offset that originates from vehicle model errors
  
      # Update Kalman filter
      angle_steers_k = float(self.C.dot(self.x_hat))
      e = steering_angle - angle_steers_k
      self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
keep keep keep keep replace replace keep keep keep keep keep
 LateralPlanner should only compute curvature (#20289)

* get curvature from planner

* no need to check active

* remove that

* remove self

* liveParams not needed

* cast

* fix test bug

* fixes

* fix ui.py

* fix radians

* update refs

* update refs

* bump cereal

* bump cereal

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> 
selfdrive/controls/lib/latcontrol_lqr.py