Is there a better way to simulate PID control in Python with Scipy's solve_ivp()?

Question

I am working on a homework problem. I'm trying to simulate a PID control in Python with Scipy's integrate.solve_ivp() function.

My method is to run the PID code within the right-hand-side of the function, using global variables and appending them to a global matrix at the end of each timestep, like so:

solution = integrate.solve_ivp(rhs, tspan, init, t_eval=teval)

Here is my code:

def rhs(dt, init):

    global old_time, omega0dot, rhs_t, omega0dotmat
    timestep = dt - old_time
    old_time = dt

    # UNPACK INITIAL
    x = init[0]
    y = init[1]
    z = init[2]
    xdot = init[3]
    ydot = init[4]
    zdot = init[5]
    alpha = init[6]
    beta = init[7]
    gamma = init[8]
    alphadot = init[9]
    betadot = init[10]
    gammadot = init[11]

    # SOLVE EQUATIONS
    (xddot, yddot, zddot, alphaddot, betaddot, gammaddot) = dynamics(k_d, k_m, x, y, z, xdot, ydot, zdot, alpha, beta, gamma, alphadot, betadot, gammadot, omega0dot)

    # CONTROL SYSTEMS
    z_des = 10

    err_z = z_des - z

    zPID = (1*err_z) + hover

    omega0dot = zPID

    rhs_t.append(dt)
    omega0dotmat.append(omega0dot)

    return [xdot, ydot, zdot, xddot, yddot, zddot, alphadot, betadot, gammadot, alphaddot, betaddot, gammaddot]

The global variables are initialized outside this function. You might notice that I am specifically trying to simulate a quadcopter, where the linear and angular motion of the quadrotor are dependent on omega0dot, which represents rotor velocity and which I am trying to control with PID.

My difficulty is with the timestep of integrate.solve_ivp(). Both the integral and derivative part of the PID control rely on the timestep, but the solve_ivp() function has a variable time step and seems to even go backwards in time sometimes, and sometimes makes no timestep (i.e. dt <= 0).

I was wondering if there was a better way to go about this PID control, or if maybe I'm interpreting the dt term in solve_ivp() wrong.

Answer 1

Let's look at a more simple system, the ubiquitous spring with dampening

    y'' + c*y' + k*y = u(t)

where u(t) could represent the force exerted by an electro-magnet (which immediately suggests ways to make the system more complicated by introducing a more realistic relation of voltage and magnetic field).

Now in a PID controller we have the error to a reference output e = yr - y and

    u(t) = kD*e'(t) + kP*e(t) + kI*integral(e(t))

To treat this with an ODE solver we immediately see that the state needs to be extended with a new component E(t) where E'(t)=e(t). The next difficulty is to implement the derivative of an expression that is not necessarily differentiable. This is achievable by avoiding to differentiate this expression at all by using a non-standard first-order implementation (where the standard would be to use [y,y',E] as state).

Essentially, collect all derived expressions in the formula in their integrated form as

    v(t)=y'(t)+c*y-kD*e(t). 

Then going back to the derivative one gets the first order system

    v'(t) = y''(t) + c*y'(t) - kD*e'(t) 
          = kP*e(t) + kI*E(t) - k*y(t)

    y'(t) = v(t) - c*y(t) + kD*e(t)
    E'(t) = e(t)

This now allows to implement the controlled system as ODE system without tricks involving a global memory or similar

def odePID(t,u,params):
    c,k,kD,kP,kI = params
    y,v,E = u
    e = yr(t)-y
    return [ v-c*y+kD*e, kP*e+kI*E-k*y, e ]

You should be able to use similar transformations of the first order system in your more complex model.