def linearize(self, x0, u0):
    '\tlinearize at a given x0, u0\n\t\t\tfor a given continuous system dxdt = f(x(t))\n\t\t\tcalculate A = ∂f/∂x, B = ∂f/∂u, g = f evaluated at x0, u0\n\t\t\tA is 6x6, B is 6x2, g is 6x1\n\t\t'
    steer = u0[1]
    psi = x0[2]
    vx = x0[3]
    vy = x0[4]
    omega = x0[5]
    vmin = 0.05
    if (vx < vmin):
        vy = 0
        omega = 0
        steer = 0
        vx = vmin
    sindelta = np.sin(steer)
    cosdelta = np.cos(steer)
    sinpsi = np.sin(psi)
    cospsi = np.cos(psi)
    (Ffy, Frx, Fry, alphaf, alphar) = self.calc_forces(x0, u0, return_slip=True)
    if self.approx:
        dFfy_dvx = (((2 * self.Cf) * ((self.lf * omega) + vy)) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFfy_dvy = ((((- 2) * self.Cf) * vx) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFfy_domega = (((((- 2) * self.Cf) * self.lf) * vx) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFrx_dvx = 0
        dFrx_dvu1 = 1
        dFry_dvx = ((((- 2) * self.Cr) * ((self.lr * omega) - vy)) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFry_dvy = ((((- 2) * self.Cr) * vx) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFry_domega = ((((2 * self.Cr) * self.lr) * vx) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFfy_delta = (2 * self.Cf)
    else:
        dFfy_dalphaf = (((self.Bf * self.Cf) * self.Df) * np.cos((self.Cf * np.arctan((self.Bf * alphaf)))))
        dFfy_dalphaf *= (1 / (1 + ((self.Bf * alphaf) ** 2)))
        dFry_dalphar = (((self.Br * self.Cr) * self.Dr) * np.cos((self.Cr * np.arctan((self.Br * alphar)))))
        dFry_dalphar *= (1 / (1 + ((self.Br * alphar) ** 2)))
        dFfy_dvx = ((dFfy_dalphaf * ((self.lf * omega) + vy)) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFfy_dvy = (((- dFfy_dalphaf) * vx) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFfy_domega = ((((- dFfy_dalphaf) * self.lf) * vx) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        if self.input_acc:
            raise NotImplementedError
        pwm = u0[0]
        dFrx_dvx = (((- self.Cm2) * pwm) - ((2 * self.Cr2) * vx))
        dFrx_dvu1 = (self.Cm1 - (self.Cm2 * vx))
        dFry_dvx = (((- dFry_dalphar) * ((self.lr * omega) - vy)) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFry_dvy = (((- dFry_dalphar) * vx) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFry_domega = (((dFry_dalphar * self.lr) * vx) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFfy_delta = dFfy_dalphaf
    f1_psi = (((- vx) * sinpsi) - (vy * cospsi))
    f1_vx = cospsi
    f1_vy = (- sinpsi)
    f2_psi = ((vx * cospsi) - (vy * sinpsi))
    f2_vx = sinpsi
    f2_vy = cospsi
    f4_vx = ((1 / self.mass) * (dFrx_dvx - (dFfy_dvx * sindelta)))
    f4_vy = ((1 / self.mass) * (((- dFfy_dvy) * sindelta) + (self.mass * omega)))
    f4_omega = ((1 / self.mass) * (((- dFfy_domega) * sindelta) + (self.mass * vy)))
    f5_vx = ((1 / self.mass) * ((dFry_dvx + (dFfy_dvx * cosdelta)) - (self.mass * omega)))
    f5_vy = ((1 / self.mass) * (dFry_dvy + (dFfy_dvy * cosdelta)))
    f5_omega = ((1 / self.mass) * ((dFry_domega + (dFfy_domega * cosdelta)) - (self.mass * vx)))
    f6_vx = ((1 / self.Iz) * (((dFfy_dvx * self.lf) * cosdelta) - (dFry_dvx * self.lr)))
    f6_vy = ((1 / self.Iz) * (((dFfy_dvy * self.lf) * cosdelta) - (dFry_dvy * self.lr)))
    f6_omega = ((1 / self.Iz) * (((dFfy_domega * self.lf) * cosdelta) - (dFry_domega * self.lr)))
    f4_u1 = dFrx_dvu1
    f4_delta = ((1 / self.mass) * (((- dFfy_delta) * sindelta) - (Ffy * cosdelta)))
    f5_delta = ((1 / self.mass) * ((dFfy_delta * cosdelta) - (Ffy * sindelta)))
    f6_delta = ((1 / self.Iz) * (((dFfy_delta * self.lf) * cosdelta) - ((Ffy * self.lf) * sindelta)))
    A = np.array([[0, 0, f1_psi, f1_vx, f1_vy, 0], [0, 0, f2_psi, f2_vx, f2_vy, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, f4_vx, f4_vy, f4_omega], [0, 0, 0, f5_vx, f5_vy, f5_omega], [0, 0, 0, f6_vx, f6_vy, f6_omega]])
    B = np.array([[0, 0], [0, 0], [0, 0], [f4_u1, f4_delta], [0, f5_delta], [0, f6_delta]])
    g = self._diffequation(None, x0, u0).reshape((- 1))
    return (A, B, g)
linearize at a given x0, u0
for a given continuous system dxdt = f(x(t))
calculate A = ∂f/∂x, B = ∂f/∂u, g = f evaluated at x0, u0
A is 6x6, B is 6x2, g is 6x1
def linearize(self, x0, u0):
    '\tlinearize at a given x0, u0\n\t\t\tfor a given continuous system dxdt = f(x(t))\n\t\t\tcalculate A = ∂f/∂x, B = ∂f/∂u, g = f evaluated at x0, u0\n\t\t\tA is 6x6, B is 6x2, g is 6x1\n\t\t'
    steer = u0[1]
    psi = x0[2]
    vx = x0[3]
    vy = x0[4]
    omega = x0[5]
    vmin = 0.05
    if (vx < vmin):
        vy = 0
        omega = 0
        steer = 0
        vx = vmin
    sindelta = np.sin(steer)
    cosdelta = np.cos(steer)
    sinpsi = np.sin(psi)
    cospsi = np.cos(psi)
    (Ffy, Frx, Fry, alphaf, alphar) = self.calc_forces(x0, u0, return_slip=True)
    if self.approx:
        dFfy_dvx = (((2 * self.Cf) * ((self.lf * omega) + vy)) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFfy_dvy = ((((- 2) * self.Cf) * vx) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFfy_domega = (((((- 2) * self.Cf) * self.lf) * vx) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFrx_dvx = 0
        dFrx_dvu1 = 1
        dFry_dvx = ((((- 2) * self.Cr) * ((self.lr * omega) - vy)) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFry_dvy = ((((- 2) * self.Cr) * vx) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFry_domega = ((((2 * self.Cr) * self.lr) * vx) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFfy_delta = (2 * self.Cf)
    else:
        dFfy_dalphaf = (((self.Bf * self.Cf) * self.Df) * np.cos((self.Cf * np.arctan((self.Bf * alphaf)))))
        dFfy_dalphaf *= (1 / (1 + ((self.Bf * alphaf) ** 2)))
        dFry_dalphar = (((self.Br * self.Cr) * self.Dr) * np.cos((self.Cr * np.arctan((self.Br * alphar)))))
        dFry_dalphar *= (1 / (1 + ((self.Br * alphar) ** 2)))
        dFfy_dvx = ((dFfy_dalphaf * ((self.lf * omega) + vy)) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFfy_dvy = (((- dFfy_dalphaf) * vx) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        dFfy_domega = ((((- dFfy_dalphaf) * self.lf) * vx) / ((((self.lf * omega) + vy) ** 2) + (vx ** 2)))
        if self.input_acc:
            raise NotImplementedError
        pwm = u0[0]
        dFrx_dvx = (((- self.Cm2) * pwm) - ((2 * self.Cr2) * vx))
        dFrx_dvu1 = (self.Cm1 - (self.Cm2 * vx))
        dFry_dvx = (((- dFry_dalphar) * ((self.lr * omega) - vy)) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFry_dvy = (((- dFry_dalphar) * vx) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFry_domega = (((dFry_dalphar * self.lr) * vx) / ((((self.lr * omega) - vy) ** 2) + (vx ** 2)))
        dFfy_delta = dFfy_dalphaf
    f1_psi = (((- vx) * sinpsi) - (vy * cospsi))
    f1_vx = cospsi
    f1_vy = (- sinpsi)
    f2_psi = ((vx * cospsi) - (vy * sinpsi))
    f2_vx = sinpsi
    f2_vy = cospsi
    f4_vx = ((1 / self.mass) * (dFrx_dvx - (dFfy_dvx * sindelta)))
    f4_vy = ((1 / self.mass) * (((- dFfy_dvy) * sindelta) + (self.mass * omega)))
    f4_omega = ((1 / self.mass) * (((- dFfy_domega) * sindelta) + (self.mass * vy)))
    f5_vx = ((1 / self.mass) * ((dFry_dvx + (dFfy_dvx * cosdelta)) - (self.mass * omega)))
    f5_vy = ((1 / self.mass) * (dFry_dvy + (dFfy_dvy * cosdelta)))
    f5_omega = ((1 / self.mass) * ((dFry_domega + (dFfy_domega * cosdelta)) - (self.mass * vx)))
    f6_vx = ((1 / self.Iz) * (((dFfy_dvx * self.lf) * cosdelta) - (dFry_dvx * self.lr)))
    f6_vy = ((1 / self.Iz) * (((dFfy_dvy * self.lf) * cosdelta) - (dFry_dvy * self.lr)))
    f6_omega = ((1 / self.Iz) * (((dFfy_domega * self.lf) * cosdelta) - (dFry_domega * self.lr)))
    f4_u1 = dFrx_dvu1
    f4_delta = ((1 / self.mass) * (((- dFfy_delta) * sindelta) - (Ffy * cosdelta)))
    f5_delta = ((1 / self.mass) * ((dFfy_delta * cosdelta) - (Ffy * sindelta)))
    f6_delta = ((1 / self.Iz) * (((dFfy_delta * self.lf) * cosdelta) - ((Ffy * self.lf) * sindelta)))
    A = np.array([[0, 0, f1_psi, f1_vx, f1_vy, 0], [0, 0, f2_psi, f2_vx, f2_vy, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, f4_vx, f4_vy, f4_omega], [0, 0, 0, f5_vx, f5_vy, f5_omega], [0, 0, 0, f6_vx, f6_vy, f6_omega]])
    B = np.array([[0, 0], [0, 0], [0, 0], [f4_u1, f4_delta], [0, f5_delta], [0, f6_delta]])
    g = self._diffequation(None, x0, u0).reshape((- 1))
    return (A, B, g)