Iterative Feedback Tuning

pidift.m

function K = pidift(K, yd, y, u, gamma, lambda, beta)
% K = pidift - Calculate new PID gains from existing gains of a discrete
%   PID controller
% 
%   K = pidift(K, yd, y, u)
% 
%       Calculate the new PID gains that will move the current system
%       response closer to the desired system response. K is a vector of
%       the current PID gains, [Kp, Ki, Kd], where the controller structure
%       is of the following form:
% 
%                          ----------
%                         |    Ki    |
%                      -->| -------- |---
%                     |   | 1 - z^-1 |   |
%                     |    ----------    | 
%                     |                  |+
%               +     |       ----    +  v
%           r ----> -------->| Kp |-----> ------> u
%                  ^          ----       ^
%                 -|                    -|
%                  |                     |
%                  y                   ----- 
%                                     | Kd  |
%                                      -----
%                                        ^
%                                        |
%                                      dy/dt
% 
%       yd is the desired system output. y is an Nx3 column vector
%       resulting from the following experiments:
% 
%           Start with a reference signal r. yd should be the desired
%           closed-loop response of the system to the reference input r.
% 
%           y1,u1 = generated by system with the reference input r
%           y2,u2 = generated by system with the reference input r - y1
%           y3,u3 = generated by system with the reference input r
% 
%       Note that y1,u3 and y3,u3 must be generated from _separate_
%       experiments.
%       
%       Once complete, y(:,1) = y1, etc.
% 
%   K = pidift(K, yd, y, u, gamma) 
% 
%       Generate new gains with a step-size of gamma used in the
%       gradient-descent of the cost funciton. gamma=0 when omitted.
% 
%   K = pidift(K, yd, y, u, gamma, lambda)
% 
%       Add input cost to the IFT cost-function. u is an Nx3 column vector
%       of input signals generated from the same experiments as y.

% 
% Source: 
%   H. Hjalmarsson et al., Iterative feedback tuning: theory and 
%   applications, Control Systems Magazine, IEEE,  vol. 18, 1998, pp.
%   26-41.
%  
% (C) 2008 Dan Miller danielmiller@ucsd.edu

% Force K into column form.
K = K(:);

if nargin < 4
    gamma = 0.1;
end
if nargin < 5
    lambda = 0;
end
if nargin < 6
    beta = ones(3,1);
end

% Extract Kp and Ki, Kd is not used directly in the cost calculations.
Kp = K(1);
Ki = K(2);

% Extract the test signals.
y1 = y(:,1);
y2 = y(:,2);
y3 = y(:,3);
u1 = u(:,1);
u2 = u(:,2);
u3 = u(:,3);

% Construct the gradient PID controller functions. These are the partial
% derivatives of the PID controller TF.
dKp = tf([1, -1], [Kp + Ki, -Kp], 1);
dKi = tf([1, 0], [Kp + Ki, -Kp], 1);
dKd = tf([1, -2, 1], [Kp + Ki, -Kp, 0], 1);

% Calculate the gradient signals.
dydKp = beta(1)*lsim(dKp, y2);
dydKi = beta(2)*lsim(dKi, y2);
dydKd = beta(3)*lsim(dKd, y2 - y3);

dudKp = lsim(dKp, u2);
dudKi = lsim(dKi, u2);
dudKd = lsim(dKd, u2 - u3);

% Calculate the gradient of the cost function for each gain.
dJ = 0;
for i = 1:length(y1)
    dJ = dJ + (y1(i) - yd(i)) * [dydKp(i); dydKi(i); dydKd(i)] ...
        + lambda * u1(i) * [dudKp(i); dudKi(i); dudKd(i)];
end
dJ = dJ/length(y1);

% Calculate the Guass-Newton-ish gradient matrix.
R = 0;
for i = 1:length(y1)
    dydrho = [dydKp(i); dydKi(i); dydKd(i)];
    dudrho = [dudKp(i); dudKi(i); dudKd(i)];
    R = R + dydrho*dydrho' + dudrho*dudrho';
end
R = R/length(y1);

% Calculate the new gains.
K = K - gamma*inv(R)*dJ;
end

pidift_demo.m

function pidift_demo
% Sampling time.
ts = 1/20;

% Construct the actual system. This is the elevator-to-pitch TF for a
% Censta taken from Roskam.
% G_act_num = [-7713.2340, -15867.0001, -908.2451];
% G_act_den = [222.0551, 1985.9525, 6262.2861, 329.8825, 180.5763];
% wn_act = 1.8;
% zeta_act = 0.7;
% G_act = c2d(tf(wn_act^2, [1, 2*wn_act*zeta_act, wn_act^2]), ts);
G_act = c2d(tf(1, [0.8, 1]), ts);

% Construct the actual closed-loop system.
kp = 0.5;
ki = 0.01;
kd = 0.001;
T_act = cl_pid(kp, ki, kd, G_act);
T_org = T_act;

% Construct the desired system. We want a critically-damped 2nd-order
% system with a natural frequency of 1.8.
% wn_des = 1.8;
% zeta_des = 0.7;
% T_des = c2d(tf(wn_des^2, [1, 2*wn_des*zeta_des, wn_des^2]), ts);
T_des = c2d(tf(1, [0.8, 1]), ts);

% Plot both the actual and desired.
figure(1);
step(T_act, T_des);
legend('Original Response', 'Desired Response');
% return
t1 = 500;
t = 0:ts:t1;
% r = ones(size(t));
r = randn(size(t));

y_des = lsim(T_des, r);
% y_des = ones(size(t));

utmp = ones(length(t),3);
gam = 0.1;

U_act = u_pid(kp, ki, kd, G_act);

% K = [kp, ki, kd];
for i = 1:10
    y1 = lsim(T_act, r);
    u1 = lsim(U_act, r);
    
    y2 = lsim(T_act, r(:) - y1(:));
    u2 = lsim(U_act, r(:) - y1(:));
    
    y3 = lsim(T_act, r);
    u3 = lsim(U_act, r);
    
    y = [y1(:), y2(:), y3(:)];
    u = [u1(:), u2(:), u3(:)];
    u = zeros(length(y1), 3);
    
    if ~all(all(isreal(u) & ~isnan(u) & ~isinf(u))) || ...
        ~all(all(isreal(y) & ~isnan(y) & ~isinf(y)))
        error('Bad numbers.')
    end
    
    K = [kp, ki, kd];
    K = pidift(K, y_des, y, u, gam, 0, [1, 0.6, 0.1]);
    
    T_act = cl_pid(K(1), K(2), K(3), G_act);
    U_act = u_pid(K(1), K(2), K(3), G_act);
    
    figure(i+1);
    step(T_org, T_act, T_des, 40);
    legend('Original Response', 'Current Response', 'Desired Response');
end

figure(i+1);
bode(T_org, T_act, T_des);
legend('Original Response', 'Current Response', 'Desired Response');


function T = cl_pid(kp, ki, kd, G)
% Function to construct a closed-loop system with a PID controller.

% Numerator and denominator of PI controller.
Npi = [kp + ki, -kp];
Dpi = [1, -1];

% Numerator and denominator of D controller.
Nd = kd/G.ts*[1, -1];
Dd = [1, 0];

num = conv(G.num{1}, Npi);
num = conv(num, Dd);

den = conv(G.den{1}, Dpi);
den = conv(den, Nd + Dd);
den = den + num;

T = tf(num, den, G.ts);

function T = u_pid(kp, ki, kd, G)
% Function to construct the closed-loop transfer function from r->u.

% Numerator and denominator of PI controller.
Npi = [kp + ki, -kp];
Dpi = [1, -1];

% Numerator and denominator of D controller.
Nd = kd/G.ts*[1, -1];
Dd = [1, 0];
    
num = conv(G.den{1}, Npi);
num = conv(num, Dd);

den = conv(G.den{1}, Dpi);
den = conv(den, Dd);
den = den + conv(G.num{1}, conv(Npi, Dd) + conv(Dpi, Nd));

T = tf(num, den, G.ts);