def _diffequation(self, t, x, u):
    '\twrite dynamics as first order ODE: dxdt = f(x(t))\n\t\t\tx is a 6x1 vector: [x, y, psi, vx, vy, omega]^T\n\t\t\tu is a 2x1 vector: [acc/pwm, steer]^T\n\t\t'
    steer = u[1]
    psi = x[2]
    vx = x[3]
    vy = x[4]
    omega = x[5]
    (Ffy, Frx, Fry) = self.calc_forces(x, u)
    dxdt = np.zeros(6)
    dxdt[0] = ((vx * np.cos(psi)) - (vy * np.sin(psi)))
    dxdt[1] = ((vx * np.sin(psi)) + (vy * np.cos(psi)))
    dxdt[2] = omega
    dxdt[3] = (((1 / self.mass) * (Frx - (Ffy * np.sin(steer)))) + (vy * omega))
    dxdt[4] = (((1 / self.mass) * (Fry + (Ffy * np.cos(steer)))) - (vx * omega))
    dxdt[5] = ((1 / self.Iz) * (((Ffy * self.lf) * np.cos(steer)) - (Fry * self.lr)))
    return dxdt
write dynamics as first order ODE: dxdt = f(x(t))
x is a 6x1 vector: [x, y, psi, vx, vy, omega]^T
u is a 2x1 vector: [acc/pwm, steer]^T
def _diffequation(self, t, x, u):
    '\twrite dynamics as first order ODE: dxdt = f(x(t))\n\t\t\tx is a 6x1 vector: [x, y, psi, vx, vy, omega]^T\n\t\t\tu is a 2x1 vector: [acc/pwm, steer]^T\n\t\t'
    steer = u[1]
    psi = x[2]
    vx = x[3]
    vy = x[4]
    omega = x[5]
    (Ffy, Frx, Fry) = self.calc_forces(x, u)
    dxdt = np.zeros(6)
    dxdt[0] = ((vx * np.cos(psi)) - (vy * np.sin(psi)))
    dxdt[1] = ((vx * np.sin(psi)) + (vy * np.cos(psi)))
    dxdt[2] = omega
    dxdt[3] = (((1 / self.mass) * (Frx - (Ffy * np.sin(steer)))) + (vy * omega))
    dxdt[4] = (((1 / self.mass) * (Fry + (Ffy * np.cos(steer)))) - (vx * omega))
    dxdt[5] = ((1 / self.Iz) * (((Ffy * self.lf) * np.cos(steer)) - (Fry * self.lr)))
    return dxdt