text: #!/usr/bin/env python3
  import numpy as np
  from numpy.linalg import solve
  from common.op_params import opParams
  
  """
  Dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"
  
  The state is x = [v, r]^T
  with v lateral speed [m/s], and r rotational speed [rad/s]
  
  The input u is the steering angle [rad]
  
  The system is defined by
  x_dot = A*x + B*u
  
  A depends on longitudinal speed, u [m/s], and vehicle parameters CP
  """
  
  
  def create_dyn_state_matrices(u, VM):
    """Returns the A and B matrix for the dynamics system
  
    Args:
      u: Vehicle speed [m/s]
      VM: Vehicle model
  
    Returns:
      A tuple with the 2x2 A matrix, and 2x1 B matrix
  
    Parameters in the vehicle model:
      cF: Tire stiffnes Front [N/rad]
      cR: Tire stiffnes Front [N/rad]
      aF: Distance from CG to front wheels [m]
      aR: Distance from CG to rear wheels [m]
      m: Mass [kg]
      j: Rotational inertia [kg m^2]
      sR: Steering ratio [-]
      chi: Steer ratio rear [-]
    """
    A = np.zeros((2, 2))
    B = np.zeros((2, 1))
    A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u)
    A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u
    A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u)
    A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u)
    B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR
    B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR
    return A, B
  
  
  def kin_ss_sol(sa, u, VM):
    """Calculate the steady state solution at low speeds
    At low speeds the tire slip is undefined, so a kinematic
    model is used.
  
    Args:
      sa: Steering angle [rad]
      u: Speed [m/s]
      VM: Vehicle model
  
    Returns:
      2x1 matrix with steady state solution
    """
    K = np.zeros((2, 1))
    K[0, 0] = VM.aR / VM.sR / VM.l * u
    K[1, 0] = 1. / VM.sR / VM.l * u
    return K * sa
  
  
  def dyn_ss_sol(sa, u, VM):
    """Calculate the steady state solution when x_dot = 0,
    Ax + Bu = 0 => x = A^{-1} B u
  
    Args:
      sa: Steering angle [rad]
      u: Speed [m/s]
      VM: Vehicle model
  
    Returns:
      2x1 matrix with steady state solution
    """
    A, B = create_dyn_state_matrices(u, VM)
    return -solve(A, B) * sa
  
  
  def calc_slip_factor(VM):
    """The slip factor is a measure of how the curvature changes with speed
    it's positive for Oversteering vehicle, negative (usual case) otherwise.
    """
    return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR)
  
  
  class VehicleModel():
    def __init__(self, CP):
      """
      Args:
        CP: Car Parameters
      """
      # for math readability, convert long names car params into short names
      self.m = CP.mass
      self.j = CP.rotationalInertia
      self.l = CP.wheelbase
      self.aF = CP.centerToFront
      self.aR = CP.wheelbase - CP.centerToFront
      self.chi = CP.steerRatioRear
  
      self.cF_orig = CP.tireStiffnessFront
      self.cR_orig = CP.tireStiffnessRear
  
      self.op_params = opParams()
      self.steer_ratio = self.op_params.get('steer_ratio', default=None)
      self.sR = CP.steerRatio
  
      self.update_params(1.0, CP.steerRatio)
  
    def update_params(self, stiffness_factor, steer_ratio):
      """Update the vehicle model with a new stiffness factor and steer ratio"""
      self.steer_ratio = self.op_params.get('steer_ratio', default=None)
  
      self.cF = stiffness_factor * self.cF_orig
      self.cR = stiffness_factor * self.cR_orig
      if type(self.steer_ratio) in [int, float]:
        self.sR = float(self.steer_ratio)
      else:
        self.sR = steer_ratio
  
    def steady_state_sol(self, sa, u):
      """Returns the steady state solution.
  
      If the speed is too small we can't use the dynamic model (tire slip is undefined),
      we then have to use the kinematic model
  
      Args:
        sa: Steering wheel angle [rad]
        u: Speed [m/s]
  
      Returns:
        2x1 matrix with steady state solution (lateral speed, rotational speed)
      """
      if u > 0.1:
        return dyn_ss_sol(sa, u, self)
      else:
        return kin_ss_sol(sa, u, self)
  
    def calc_curvature(self, sa, u):
      """Returns the curvature. Multiplied by the speed this will give the yaw rate.
  
      Args:
        sa: Steering wheel angle [rad]
        u: Speed [m/s]
  
      Returns:
        Curvature factor [1/m]
      """
      return self.curvature_factor(u) * sa / self.sR
  
    def curvature_factor(self, u):
      """Returns the curvature factor.
      Multiplied by wheel angle (not steering wheel angle) this will give the curvature.
  
      Args:
        u: Speed [m/s]
  
      Returns:
        Curvature factor [1/m]
      """
      sf = calc_slip_factor(self)
      return (1. - self.chi) / (1. - sf * u**2) / self.l
  
    def get_steer_from_curvature(self, curv, u):
      """Calculates the required steering wheel angle for a given curvature
  
      Args:
        curv: Desired curvature [1/m]
        u: Speed [m/s]
  
      Returns:
        Steering wheel angle [rad]
      """
  
      return curv * self.sR * 1.0 / self.curvature_factor(u)
  
    def get_steer_from_yaw_rate(self, yaw_rate, u):
      """Calculates the required steering wheel angle for a given yaw_rate
  
      Args:
        yaw_rate: Desired yaw rate [rad/s]
        u: Speed [m/s]
  
      Returns:
        Steering wheel angle [rad]
      """
      curv = yaw_rate / u
      return self.get_steer_from_curvature(curv, u)
  
    def yaw_rate(self, sa, u):
      """Calculate yaw rate
  
      Args:
        sa: Steering wheel angle [rad]
        u: Speed [m/s]
  
      Returns:
        Yaw rate [rad/s]
      """
      return self.calc_curvature(sa, u) * u
  
  
  if __name__ == '__main__':
    import math
    from selfdrive.car.honda.interface import CarInterface
    from selfdrive.car.honda.values import CAR
  
    CP = CarInterface.get_params(CAR.CIVIC)
    VM = VehicleModel(CP)
    print(VM.yaw_rate(math.radians(20), 10.))
  