Cannot get 3 DOF bicycle model to run correctly

Question

I'm trying to simulate a vehicle using a dynamic bicycle model but I cannot seem to get it working. If I set a constant steering angle the lateral velocity grows exponentially and creates impossible results.

a = 0.34284
b = 0.40716
m = 155
I = 37.29

def f_DynBkMdl(x, y, delta, theta, dt, states):
    dtheta = states[0]
    vlat = states[1]
    vlon = states[2]

    if delta > math.radians(180):
        delta = delta - math.radians(360)

    if delta<0:
        j = 1
    else:
        j = 0
    if dtheta<0:
        q = 1
    else:
        q = 0

    dtheta = abs(dtheta)
    delta = abs(delta)

    sf = delta - (a*dtheta)/vlon
    ff = 30.77*math.degrees(sf)
    pf = 0

    sr = (b*dtheta)/vlon
    fr = 30.77*math.degrees(sr)       
    pr = 0

    if j == 1:
        fr = -fr
        ff = -ff
    if q == 1:
        dtheta = -dtheta

    theta = theta%math.radians(360)

    ddtheta = (a*pf*delta + a*ff - b*fr)/I
    dvlat = (pf*delta + ff + fr)/m - vlon*dtheta
    dvlon = (pf + pr - ff*delta)/m - vlat*dtheta

    dx = -vlat*np.sin(theta) + vlon*np.cos(theta)
    dy = vlat*np.cos(theta) + vlon*np.sin(theta)

    theta = theta + dtheta*dt + (1/2)*ddtheta*dt**2
    dtheta = dtheta + ddtheta*dt
    vlat = vlat + dvlat*dt
    vlon = vlon + dvlon*dt
    vabs = np.sqrt(vlat**2 + vlon**2)
    x = x + dx*dt
    y = y + dy*dt

    states = [dtheta, vlat, vlon]

    array = np.array([x, y, theta, vabs, states])
    return array

With a and b being the distance between the front and rear axle to the vehicle's centre of gravity, m being the mass and I the inertia. x and y are the global position and theta is the heading with delta being the steering angle.

I obtained my equations from this document https://vtechworks.lib.vt.edu/bitstream/handle/10919/36615/Chapter2a.pdf under the headings 2.3.1, 2.3.2 and 2.3.3. I used a simplified tyre model and assumed infinite friction so the friction circle is not required.

Is there something I am missing to make this work?

Answer 1

I think this question doesn't belong here, it is more suitable to the stackoverflow. To the problem now, first of all I assume you are making a mistake by mixing degrees with radians. In this part of the code

 if delta > 180:
        delta = delta - math.radians(360)

delta seems to be in degrees and then you are trying to normalize it by subtracting radians (hint you might get angles that are even bigger than 360 so you should make a loop to check it out).

While here:

  sf = delta - (a*dtheta)/vlon

you are using it as a number(radians).

Please check it out, and upload the source where you got the equations from because it is realy hard to follow your code.