(%i1) load ( "utilities.wxm" ) ;

\[\tag{%o1} utilities.wxm\]

(%i2) simp : false ;

\[\tag{%o2} \mbox{false}\]

1 Definintion

The PID transfer function. e(s)=r(s)-y(s) is the input error signal with r(s) the setpoint and y(s) the sensor readings. u(s) is the output to the actuator.
(%i3) tf : u ( s ) / e ( s ) = ( Kp + Ki / s + Kd · s ) · ( 1 / ( Tf · s + 1 ) ) ;

\[\tag{%o3} \frac{\operatorname{u}(s)}{\operatorname{e}(s)}=\left( \mathit{Kp}+\frac{\mathit{Ki}}{s}+\mathit{Kd} s\right) \, \frac{1}{\mathit{Tf} s+1}\]

Select integration method: backward_euler or bilinear
(%i4) num_integrate : bilinear ;

\[\tag{%o4} \mathit{bilinear}\]

(%i5) simp : true ;

\[\tag{%o5} \mbox{true}\]

(%i6) num_integrate ( diff ( y ( t ) , t ) = f ( y ( t ) , t ) ) ;

\[\tag{%o6} {y_k}=\frac{h \operatorname{f}\left( {y_k}\operatorname{,}h k\right) +2 {y_{k-1}}+h \operatorname{f}\left( {y_{k-1}}\operatorname{,}h k-h\right) }{2}\]

2 Realization

The 2nd order differential equation is split into 1st order equations by substituting state variables.
Partial fractions allow the separation of the integrator and the low pass filter into distinct states.
(%i7) tffr : partfrac ( tf · denom ( lhs ( tf ) ) , s ) ;

\[\tag{%o7} \operatorname{u}(s)=-\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) \operatorname{e}(s)}{\mathit{Tf}\, \left( \mathit{Tf} s+1\right) }+\frac{\mathit{Ki} \operatorname{e}(s)}{s}+\frac{\mathit{Kd} \operatorname{e}(s)}{\mathit{Tf}}\]

The pole is assigned to the D(s) variable.
(%i8) tfD : D ( s ) = part ( tffr , 2 , 1 ) ;

\[\tag{%o8} \operatorname{D}(s)=-\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) \operatorname{e}(s)}{\mathit{Tf}\, \left( \mathit{Tf} s+1\right) }\]

The integrator is assigned to the I(s) variable.
(%i9) tfI : I ( s ) = part ( tffr , 2 , 2 ) ;

\[\tag{%o9} \operatorname{I}(s)=\frac{\mathit{Ki} \operatorname{e}(s)}{s}\]

The states are substituted into the output equation. It contains no further state.
(%i10) tfY : subst ( [ reverse ( tfD ) , reverse ( tfI ) ] , tffr ) ;

\[\tag{%o10} \operatorname{u}(s)=\frac{\mathit{Kd} \operatorname{e}(s)}{\mathit{Tf}}+\operatorname{I}(s)+\operatorname{D}(s)\]

Inverse laplace transformation to get ordinary differential equations.
(%i11) od : map ( ilp , [ tfD , tfI , tfY ] ) ;

\[\tag{%o11} [\frac{d}{d t} \operatorname{D}(t)=-\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) \operatorname{e}(t)+\mathit{Tf} \operatorname{D}(t)}{{{\mathit{Tf}}^{2}}}\operatorname{,}\frac{d}{d t} \operatorname{I}(t)=\mathit{Ki} \operatorname{e}(t)\operatorname{,}\operatorname{u}(t)=\frac{\mathit{Kd} \operatorname{e}(t)+\mathit{Tf} \operatorname{I}(t)+\mathit{Tf} \operatorname{D}(t)}{\mathit{Tf}}]\]

The 1st order differential equations are discretized and solved with the selected integration method.
h is the sampling time.
(%i12) pidk : [ num_integrate ( od [ 1 ] ) , num_integrate ( od [ 2 ] ) , discretize ( od [ 3 ] ) ] ;

\[\tag{%o12} [{D_k}=-\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h\, {e_k}+\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h\, {e_{k-1}}+\left( \mathit{Tf} h-2 {{\mathit{Tf}}^{2}}\right) \, {D_{k-1}}}{\mathit{Tf} h+2 {{\mathit{Tf}}^{2}}}\operatorname{,}{I_k}=\frac{\mathit{Ki} h\, {e_k}+\mathit{Ki} h\, {e_{k-1}}+2 {I_{k-1}}}{2}\operatorname{,}{u_k}=\frac{\mathit{Kd}\, {e_k}+\mathit{Tf}\, {I_k}+\mathit{Tf}\, {D_k}}{\mathit{Tf}}]\]

Define the variables for the state transition matrix.
(%i13) vin : [ D [ k − 1 ] , I [ k − 1 ] , e [ k ] , e [ k − 1 ] ] ;

\[\tag{%o13} [{D_{k-1}}\operatorname{,}{I_{k-1}}\operatorname{,}{e_k}\operatorname{,}{e_{k-1}}]\]

These are the variables that need to be solved.
(%i14) vout : map ( lhs , pidk ) ;

\[\tag{%o14} [{D_k}\operatorname{,}{I_k}\operatorname{,}{u_k}]\]

This is already the discrete state space representation of the PID
(%i15) − augcoefmatrix ( solve ( pidk , vout ) [ 1 ] , vin ) ;

\[\tag{%o15} \begin{pmatrix}-\frac{h-2 \mathit{Tf}}{h+2 \mathit{Tf}} & 0 & -\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h}{\mathit{Tf} h+2 {{\mathit{Tf}}^{2}}} & -\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h}{\mathit{Tf} h+2 {{\mathit{Tf}}^{2}}} & -{D_k}\\ 0 & 1 & \frac{\mathit{Ki} h}{2} & \frac{\mathit{Ki} h}{2} & -{I_k}\\ -\frac{h-2 \mathit{Tf}}{h+2 \mathit{Tf}} & 1 & \frac{\mathit{Ki}\, {{h}^{2}}+2 \mathit{Kp} h+4 \mathit{Kd}}{2 h+4 \mathit{Tf}} & \frac{\mathit{Ki}\, \mathit{Tf}\, {{h}^{2}}+\left( 2 \mathit{Kp}\, \mathit{Tf}-2 \mathit{Kd}\right) h}{2 \mathit{Tf} h+4 {{\mathit{Tf}}^{2}}} & -{u_k}\end{pmatrix}\]

But for further simplification we prefer this form
(%i16) M0 : − augcoefmatrix ( pidk , vin ) ;

\[\tag{%o16} \begin{pmatrix}-\frac{h-2 \mathit{Tf}}{h+2 \mathit{Tf}} & 0 & -\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h}{\mathit{Tf} h+2 {{\mathit{Tf}}^{2}}} & -\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h}{\mathit{Tf} h+2 {{\mathit{Tf}}^{2}}} & -{D_k}\\ 0 & 1 & \frac{\mathit{Ki} h}{2} & \frac{\mathit{Ki} h}{2} & -{I_k}\\ 0 & 0 & \frac{\mathit{Kd}}{\mathit{Tf}} & 0 & \frac{\mathit{Tf}\, {I_k}+\mathit{Tf}\, {D_k}}{\mathit{Tf}}-{u_k}\end{pmatrix}\]

Define constants for the coefficients.
(%i17) constlist : delete ( false , list_matrix_entries ( genmatrix ( lambda ( [ r , c ] , if numberp ( M0 [ r , c ] ) then false else concat ( [ A , B , C ] [ r ] , c ) = M0 [ r , c ] ) , length ( vout ) , length ( vin ) ) ) ) ;

\[\tag{%o17} [\mathit{A1}=-\frac{h-2 \mathit{Tf}}{h+2 \mathit{Tf}}\operatorname{,}\mathit{A3}=-\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h}{\mathit{Tf} h+2 {{\mathit{Tf}}^{2}}}\operatorname{,}\mathit{A4}=-\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h}{\mathit{Tf} h+2 {{\mathit{Tf}}^{2}}}\operatorname{,}\mathit{B3}=\frac{\mathit{Ki} h}{2}\operatorname{,}\mathit{B4}=\frac{\mathit{Ki} h}{2}\operatorname{,}\mathit{C3}=\frac{\mathit{Kd}}{\mathit{Tf}}]\]

(%i18) block ( [ ratfac : true ] , ratsimp ( constlist ) ) ;

\[\tag{%o18} [\mathit{A1}=-\frac{h-2 \mathit{Tf}}{h+2 \mathit{Tf}}\operatorname{,}\mathit{A3}=-\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h}{\mathit{Tf}\, \left( h+2 \mathit{Tf}\right) }\operatorname{,}\mathit{A4}=-\frac{\left( \mathit{Ki}\, {{\mathit{Tf}}^{2}}-\mathit{Kp}\, \mathit{Tf}+\mathit{Kd}\right) h}{\mathit{Tf}\, \left( h+2 \mathit{Tf}\right) }\operatorname{,}\mathit{B3}=\frac{\mathit{Ki} h}{2}\operatorname{,}\mathit{B4}=\frac{\mathit{Ki} h}{2}\operatorname{,}\mathit{C3}=\frac{\mathit{Kd}}{\mathit{Tf}}]\]

Substitute them back into the matrix
(%i19) M1 : subst ( map ( reverse , constlist ) , M0 ) ;

\[\tag{%o19} \begin{pmatrix}\mathit{A1} & 0 & \mathit{A3} & \mathit{A3} & -{D_k}\\ 0 & 1 & \mathit{B3} & \mathit{B3} & -{I_k}\\ 0 & 0 & \mathit{C3} & 0 & \frac{\mathit{Tf}\, {I_k}+\mathit{Tf}\, {D_k}}{\mathit{Tf}}-{u_k}\end{pmatrix}\]

Rebuild the array of equations with the coefficients substituted
(%i20) M2 : M1 . append ( vin , [ 1 ] ) ;

\[\tag{%o20} \begin{pmatrix}\mathit{A3}\, {e_k}-{D_k}+\mathit{A3}\, {e_{k-1}}+\mathit{A1}\, {D_{k-1}}\\ \mathit{B3}\, {e_k}-{I_k}+\mathit{B3}\, {e_{k-1}}+{I_{k-1}}\\ -{u_k}+\mathit{C3}\, {e_k}+\frac{\mathit{Tf}\, {I_k}+\mathit{Tf}\, {D_k}}{\mathit{Tf}}\end{pmatrix}\]

These are equations to implement for the PID
(%i21) f0 : flatten ( map ( solve , list_matrix_entries ( M2 ) , vout ) ) ;

\[\tag{%o21} [{D_k}=\mathit{A3}\, {e_k}+\mathit{A3}\, {e_{k-1}}+\mathit{A1}\, {D_{k-1}}\operatorname{,}{I_k}=\mathit{B3}\, {e_k}+\mathit{B3}\, {e_{k-1}}+{I_{k-1}}\operatorname{,}{u_k}=\mathit{C3}\, {e_k}+{I_k}+{D_k}]\]

3 Initialization

Since this is a initial value problem some initialization for the state variables D[k] and I[k] is needed.

3.1 Steady State Initialization

If you only have the actuator setting you can use steady state initialization assuming that the system is in equilibrium.
Under the condition that the state variables must not change,
I[k]=I[k-1],D[k]=D[k-1], e[k]=e[k-1], only the steady state output value u[k] is needed.
(%i22) solve ( append ( f0 , [ I [ k ] = I [ k − 1 ] , D [ k ] = D [ k − 1 ] , e [ k ] = e [ k − 1 ] ] ) , [ I [ k ] , D [ k ] , D [ k − 1 ] , I [ k − 1 ] , e [ k ] , e [ k − 1 ] ] ) ;

\[\tag{%o22} [[{I_k}={u_k}\operatorname{,}{D_k}=0\operatorname{,}{D_{k-1}}=0\operatorname{,}{I_{k-1}}={u_k}\operatorname{,}{e_k}=0\operatorname{,}{e_{k-1}}=0]]\]

3.2 Partial Re-Initialization

Partial re-initialization can be used to avoid jumps in the output signal when the PID parameters have been
modified or the actuator value u[k] is different because the system is currently not controlled by the PID.
It keeps the state of the D[k] variable which represents the low pass filter that will decay with the rate of the new parameters and
adjust the I[k] variable to match the last data point (e[k], u[k]).
(%i23) solve ( f0 , [ I [ k ] , I [ k − 1 ] , D [ k − 1 ] ] ) ;

\[\tag{%o23} [[{I_k}={u_k}-\mathit{C3}\, {e_k}-{D_k}\operatorname{,}{I_{k-1}}={u_k}+\left( -\mathit{C3}-\mathit{B3}\right) \, {e_k}-{D_k}-\mathit{B3}\, {e_{k-1}}\operatorname{,}{D_{k-1}}=-\frac{\mathit{A3}\, {e_k}-{D_k}+\mathit{A3}\, {e_{k-1}}}{\mathit{A1}}]]\]

3.3 Initialization by Input and Output Data

The state variables D[k] and I[k] can be initialized by supplying two IO data points (e[k], u[k]) and (e[k-1], u[k-1])
(%i24) f1 : subst ( k − 1 , k , f0 ) ;

\[\tag{%o24} [{D_{k-1}}=\mathit{A3}\, {e_{k-1}}+\mathit{A3}\, {e_{k-2}}+\mathit{A1}\, {D_{k-2}}\operatorname{,}{I_{k-1}}=\mathit{B3}\, {e_{k-1}}+\mathit{B3}\, {e_{k-2}}+{I_{k-2}}\operatorname{,}{u_{k-1}}=\mathit{C3}\, {e_{k-1}}+{I_{k-1}}+{D_{k-1}}]\]

(%i25) ratsimp ( solve ( append ( f0 , f1 ) , [ D [ k ] , I [ k ] , D [ k − 1 ] , I [ k − 1 ] , D [ k − 2 ] , I [ k − 2 ] ] ) [ 1 ] ) ;

\[\tag{%o25} [{D_k}=\frac{\mathit{A1}\, {u_k}+\left( -\mathit{A1}\, \mathit{C3}-\mathit{A1}\, \mathit{B3}-\mathit{A3}\right) \, {e_k}-\mathit{A1}\, {u_{k-1}}+\left( \mathit{A1}\, \mathit{C3}-\mathit{A1}\, \mathit{B3}-\mathit{A3}\right) \, {e_{k-1}}}{\mathit{A1}-1}\operatorname{,}{I_k}=-\frac{{u_k}+\left( -\mathit{C3}-\mathit{A1}\, \mathit{B3}-\mathit{A3}\right) \, {e_k}-\mathit{A1}\, {u_{k-1}}+\left( \mathit{A1}\, \mathit{C3}-\mathit{A1}\, \mathit{B3}-\mathit{A3}\right) \, {e_{k-1}}}{\mathit{A1}-1}\operatorname{,}{D_{k-1}}=\frac{{u_k}+\left( -\mathit{C3}-\mathit{B3}-\mathit{A3}\right) \, {e_k}-{u_{k-1}}+\left( \mathit{C3}-\mathit{B3}-\mathit{A3}\right) \, {e_{k-1}}}{\mathit{A1}-1}\operatorname{,}{I_{k-1}}=-\frac{{u_k}+\left( -\mathit{C3}-\mathit{B3}-\mathit{A3}\right) \, {e_k}-\mathit{A1}\, {u_{k-1}}+\left( \mathit{A1}\, \mathit{C3}-\mathit{B3}-\mathit{A3}\right) \, {e_{k-1}}}{\mathit{A1}-1}\operatorname{,}{D_{k-2}}=\frac{{u_k}+\left( -\mathit{C3}-\mathit{B3}-\mathit{A3}\right) \, {e_k}-{u_{k-1}}+\left( \mathit{C3}-\mathit{B3}-\mathit{A1}\, \mathit{A3}\right) \, {e_{k-1}}+\left( 1-\mathit{A1}\right) \, \mathit{A3}\, {e_{k-2}}}{{{\mathit{A1}}^{2}}-\mathit{A1}}\operatorname{,}{I_{k-2}}=-\frac{{u_k}+\left( -\mathit{C3}-\mathit{B3}-\mathit{A3}\right) \, {e_k}-\mathit{A1}\, {u_{k-1}}+\left( \mathit{A1}\, \mathit{C3}+\left( \mathit{A1}-2\right) \, \mathit{B3}-\mathit{A3}\right) \, {e_{k-1}}+\left( \mathit{A1}-1\right) \, \mathit{B3}\, {e_{k-2}}}{\mathit{A1}-1}]\]

Created with wxMaxima.