Why does Simulink generate this code for a PID controller?

Question

For the Simulink PID Controller model

The Simulink generated code (rewrite for better understanding) is:

#define PERIOD 0.005
double PID_step(double err,double P,double I,double D,double N){
  static double Integrator,Filter;
  double POut,IOut,DOut,Out;

  POut= err * P;
  DOut = ((err * D) - Filter) * N;//LineA
  Out = (POut + Integrator) + DOut;
  Integrator += (err * I) * PERIOD;
  Filter += PERIOD * DOut;//LineB
  return Out;
}

Line A and B are the code for the differential part, err is the input and DOut is the output. The Z-domain transfer function is $$\frac{DN}{1+\frac{NT_s}{z-1}}.$$

I want to derive the code from the transfer function.

Let $$\frac{Y(z)}{X(z)}=\frac{DN}{1+\frac{NT_s}{z-1}},$$ then $$zY(z)+(NT_s-1)Y(z)=DN(zX(z)-X(z))$$

Use Z-transform formula $$z^{-n}Y(z) \leftrightarrow y[k-n],$$ we have $$y[k+1]=DN(x[k+1]-x[k])-(NT_s-1)y[k]$$

Here x is the input and y is the output.

So my code for the PID differential part is

double y0=0.0;
double Ts=0.005;
double PID_DifferentialPart(double x,double D,double N){
  static double x_last,y_last;
  static int first=1;
  double y;
  if(first){
    first=0;
    x_last=x;
    y=y0;
  } else {
    y=D*N*(x-x_last)-(N*Ts-1.0)*y_last;
  }
  y_last=y;
  return y;
}

Using the same input (e.g. input sequence {0 2 3 4 2.5}, with D=2.0,N=100.0,Ts=0.005), the outputs are different. The output of my code is {0 400 800 1200 1100}. Output of Simulink code is {0 400 400 400 -100}. Where did I go wrong?

Answer 1

I'm not sur exactly where you went wrong because your haven't fully explained your approach but for the derivate component of the PID, you should convert the C++ lines to this :

$$ D_{out}[n] = N(err[n]D -F[n-1]) \\ F[n] = F[n-1] + T_s*D_{out}[n] \\\\$$

In the z-domain : $$ F(1-z^{-1}) = T_s*D_{out} \\ F = \frac{T_s*D_{out}}{1-z^{-1}}\\ D_{out} = N(err * D-Fz^{-1}) \\ D_{out} = N(err*D-\frac{T_sD_{out}}{1-z^{-1}}z^{-1}) \\ D_{out}(1+\frac{NT_sz^{-1}}{1-z^{-1}}) = N(err*D) \\ \frac{D_{out}}{err} = \frac{N*D}{1+\frac{NT_sz^{-1}}{1-z^{-1}}} \\ \frac{D_{out}}{err} = \frac{N*D}{1+\frac{NT_s}{z-1}} $$

Edit :

You posted C++ code. Why is x_last only updated once? It needs to be updated every period. Try my solution below

double y0=0.0;
double Ts=0.005;
double PID_DifferentialPart(double x,double D,double N){
  static double x_last,y_last;
  static int first=1;
  double y;
  if(first){
    first=0;
    x_last = 0;
    y=y0;
  } else {
    y=D*N*(x-x_last)-(N*Ts-1.0)*y_last;
  }
  y_last=y;
  x_last=x;
  return y;
}