% PID controller with 2nd order filtering of measured signal, anti-windup
% from Astrom and Hagglund "Advanced PID Control"  2006 page 428
% Used defined constants
Tf = 1;      % Derivative filter time constant
h = .1;      % Sample period (seconds)
K = 1;       % Constant for P term of PID
Ti = 1;       % Time constant for I term of PID
Td = 1;      % Time constant for D term of PID
Tt = 1;       % Anti-windup time constant
B = 1;        % Setpoint weight
Ysp = 0;      % Desired setpoint
uLow = -1000;     %minimum possible output
uHigh = 1000;      %maximum possible output

%Pre-computed constants for PID algorithm
den = (Tf * Tf) + (2 * h * Tf) + (2 * h * h);   % denominator of measured signal filter terms
p1 = Tf * Tf / den;                                     % first part of state variable x2 equation
p2 = 2 * h * h / den;                                  % second part of state variable x1 equation
p3 = K * h / Ti;                                          % integral gain
p4 = K * Td / h;                                         % derivative gain
p5 = h / Tt;                                               % anti-windup gain

I = 0;
y1 = 0;
y2 = 0;
%If we want bumpless parameter changes fill in the new K and B
Kold = K;
Knew = K;
Bold = B;
Bnew = B;
I = I + Kold * (Bold * Ysp - y1) - Knew * (Bnew * Ysp - y1);

%PID Algorithm
for i =1:1000
    y = pv(i);                                            % read the process variable
    y2 = p1 * y2 + p2 * (y - y1);                % update filter state x2
    y1 = y1 + y2;                                      % update filter state x1
    v = K * (B * Ysp - y1) - p4 * y2 + I;       % compute nominal output
    if (v < uLow)                                       % limit the nominal output to desired range 
        u(i) = uLow;
    elseif (v > uHigh)
        u(i) = uHigh;
    else
        u(i) = v;
    end
    I = I + p3 * (Ysp - y1) + p5 * (u(i) - v);     % update the integral
end
