# Runge-Kutta for PID and system in separate calculations without filter

I need to calculate a closed-loop system in Python; specifically, obtain the PID response and then use the output to obtain the system response sample-by-sample with my own loop.

For this, I am using a fourth-order Runge-Kutta with an adaptitive step size to solve the state space representation. So far, the only way I managed to get a good aproximate response was by adding a low-pass filter with the PID:

In transfer function form, my PID without filter:

$$G(s)_c = K_p + \displaystyle\frac{K_i}{s} + \displaystyle\frac{K_dN}{1+ \displaystyle\frac{N}{s}}$$

My PID with the filter:

$$G(s)_c = \displaystyle\frac{1}{0.1s + 1}\left(K_p + \displaystyle\frac{K_i}{s} + \displaystyle\frac{K_dN}{1+ \displaystyle\frac{N}{s}}\right)$$

With an example system:

$$G(s) = \displaystyle\frac{1}{s^2 + s + 1}$$,

where $$K_p = K_i = K_d = 1$$ and $$N = 100$$

Result with the filter:

and without the filter:

The expected response should resemble the first image, but the filter adds tiny changes to the response. If I calculate PID + system in one transfer function, I get the expected results and I don't need the filter (probably because the system act as a filter), but, like i said, the requirement is to make the calculation of the PID separately.

I can also obtain the expected response if I increase the relative tolerance and absolute tolerance of the RK4, but the amount of increase in the # of steps is unacceptable, from 890 to 4269. The system itself does not matter, could be anything.

Any ideas on how could I get an accurate result without the filter or a big increase in number of steps?

the complete python code:

import numpy as np
import control as ctrl
from matplotlib import pyplot as plt
import time

def norm(x):
return np.linalg.norm(x) / x.size**0.5

def runge_kutta(ss, x, h, inputValue):
k1 = h * (ss.A * x + ss.B * inputValue)
k2 = h * (ss.A * (x + k1/2) + ss.B * inputValue)
k3 = h * (ss.A * (x + k2/2) + ss.B * inputValue)
k4 = h * (ss.A * (x+k3) + ss.B * inputValue)

x = x + (1/6) * (k1 + k2*2 + k3*2 + k4)
y = ss.C * x + ss.D * inputValue
return y.item(), x

N = 100
kp = 1
ki = 1
kd = 1

pid = ctrl.tf2ss(
ctrl.TransferFunction([1], [0.1, 1]) *
ctrl.TransferFunction([N*kd + kp, N*kp + ki, N * ki], [1, N, 0]))

x_pidB = np.zeros_like(pid.B)
x_pidS = np.zeros_like(pid.B)

system = ctrl.tf2ss(ctrl.TransferFunction([1], [1, 1, 1]))

min_step_decrease = 0.2
max_step_increase = 5
h_ant = 0.0001
rtol = 1e-3
atol = 1e-6
Time = 0
tbound = 30
sp = 1
output = [0]
Time_out = [0]
yb = 0
sf1 = 0.9
sc_t = [0]
start = time.time()
counter = 0

while Time < tbound:
error = sp - yb
while True:
counter += 1
if Time + h_ant >= tbound:
h_ant = tbound - Time
ypids, x_pidSn = runge_kutta(pid, x_pidS, h_ant, error)
else:
ypidb, x_pidBn = runge_kutta(pid, x_pidB, h_ant, error)
ypids, x_pidSn = runge_kutta(pid, x_pidS, h_ant / 2, error)
ypids, x_pidSn = runge_kutta(pid, x_pidSn, h_ant / 2, error)

scale = atol + rtol * (np.abs(x_pidSn) + np.abs(x_pidBn)) / 2
delta1 = np.abs(x_pidBn - x_pidSn)
error_norm = norm(delta1 / scale)

if error_norm == 0:
h_est = h_ant * max_step_increase
elif error_norm < 1:
h_est = h_ant * min(max_step_increase,
max(1, sf1 * error_norm**(-1 / (4+1))))
else:
h_ant = h_ant * max(min_step_decrease, sf1 * error_norm**(-1 / (4+1)))
continue

break

sc_t.append(ypids)
output.append(yb)
Time += h_ant
Time_out.append(Time)

h_ant = h_est
x_pidB = x_pidSn
x_pidS = x_pidSn

print(f'Number of calculations:{counter}')
print(f'Time:{time.time() - start:.3f}')

fig = plt.figure(212)