Writing a filtered derivative term of the PID controller into the C++ code

Question

Everywhere I look, be it a PID, lead, lag control or anything else, there are Simulink schematics with transfer functions. This is all nice for system response simulation, however currently I have to implement a PID control with a filtered derivative term into a strictly certified medical device, which is controlled by a microchip with a C++ code controlling it.

Now, if we take the PID controller in its frequency domain as

$$C(s) = k_\mathrm{P} + \frac{k_\mathrm{I}}{s} + k_\mathrm{D}s = \frac{y(s)}{e(s)},$$

we can implement that as

$$y(t) = k_\mathrm{P}e(t) + k_\mathrm{I}\int_{0}^{t}e(\tau)d\tau + k_\mathrm{D}\dot{e}(t), \quad e(0) = 0,$$

which we can write into pseudo code as

integral += error*dt
derivative = (error - prevError) / dt
y = kp*error + ki*integral + kd*derivative
prevError = error

However, now we take the filtered PID control as

$$C(s) = k_\mathrm{P} + \frac{k_\mathrm{I}}{s} + k_\mathrm{D}\frac{sN}{s + N}$$

The best I can think of is to create inverse Laplace as

$$y(t) = k_\mathrm{P}e(t) + k_\mathrm{I}\int_{0}^{t}e(\tau)d\tau + k_\mathrm{D}\left( N\delta(t) - N^2e^{-Nt} \right)e(t), \quad e(0) = 0,$$

but what does that really represent? Integrating via $dt$ is one thing, but all I can see in the $e^{-Nt}$ is the fact that after few seconds, no matter what I do, I am going to have just another proportional constant. Should I reset the time at some point? And we haven't even written that into C++ yet.

What is the correct approach?

Answer 1

Well, I have no idea whether this is correct, but the only thing that came to my mind after the night's sleep and a morning shower is the following:

My inverse Laplace is wrong, because I did not account for the error input $e(t)$ that is time variant. Therefore, if we look at the derivative part of the transfer function of the controller, we can write

$$C_\mathrm{D}(s) = \frac{sN}{s + N}.$$

Now we take whatever the error input there is and recreate the output as

$$y_\mathrm{D}(s) = \frac{sN}{s + N}e(s) \implies sy_\mathrm{D}(s) + Ny_\mathrm{D}(s) = Nse(s),$$

which can be transformed into time domain as

$$\dot{y}_\mathrm{D}(t) + Ny_\mathrm{D}(t) = N\dot{e}(t).$$

The solution for this differential equation is

$$y_\mathrm{D}(t) = \mathrm{constant} \times e^{-Nt} + e^{-Nt}\int_0^tN\dot{e}(\tau)e^{N\tau}d\tau, \quad y_\mathrm{D}(0) = 0, \quad e(0) = e_0.$$

This could be implemented into the code, but I don't fancy the absolute time $t$ being there, because we are talking embedded system with finite memory and about 120 MHz clock.

The other option is to discretize the differential equation as

$$\frac{y_\mathrm{D}^n - y_\mathrm{D}^{n - 1}}{\Delta t} + Ny_\mathrm{D}^{n} = N\frac{e^n - e^{n - 1}}{\Delta t}, \quad n \in \mathbb{N},$$

therefore

$$y_\mathrm{D}^n(1 + N\Delta t) = N\left( e^n - e^{n - 1} \right) + y_\mathrm{D}^{n - 1}, \quad y_\mathrm{D}^0 = 0, \quad e^0 = e_0,$$

where $n$ is the time-step, not power to the $n$. From this, we are able to compute the $y_\mathrm{D}^n$, sum it to the other two constants of the controller and feed that to the input of the actuators.

Well, the problem is that I am currently facing the fact that an embedded system will have to solve a differential equation, real-time. I don't know whether this is the correct approach in the industry, so I would be glad if anyone could point out a different solution. The problems I see with this are many - discretized equations are basically forward Euler, may this cause some problems? Shouldn't Runge-Kutta be used?

Answer 2

Please check the following reference http://www.ifac-control.org/publications/list-of-professional-briefs/pb_wittenmark_etal_final.pdf, especially page 50. I think this should help you completely.