pidIE.m 2.09 KB
Newer Older
Kristian Soltesz's avatar
Kristian Soltesz committed
1
function [K,p,w] = pidIE(P,Ms,Mt,Mks,p,w)
2
3
4
5
6
7
8
9
10
11
12
% Computes PID controller that maximizes the integral gain subject to
% constraints on the magnitude of the sensitivity, complementary
% sensitivity and noise sensitivity functions.
% The length of p decides if the controller is of PI or PID type.
%
% K(s) = kp + ki/s + kd*s
%
% P   - LTI process model.
% Ms  - Maximum allowed sensitivity.
% Mt  - Maximum allowed complementary sensitivity.
% Mks - Maxium allowed noise sensitivity.
13
14
% p   - Vector of controller parameters. Length must be 2 (PI)
%       or 3 (PID). Controller must stabilize plant.
Kristian Soltesz's avatar
Kristian Soltesz committed
15
% w   - angular freqency grid. Use [] for default grid.
16
17
%
% Downloaded from git@gitlab.control.lth.se:kristian/PIDopt.git
18
% See the git repo for further documentation.
19

20
% Frequency grid (heuristic) - change as needed
Kristian Soltesz's avatar
Kristian Soltesz committed
21
22
23
if isempty(w)
    w = defaultGrid(P);
end
24

25
% Frequency response
26
27
28
P = squeeze(freqresp(balreal(P),w));

% Constraint represented as circles with centresa and radii :
29
30
cs = -1;                % Ms center
rs = 1./Ms;             % Ms radius
31
ct = -Mt.^2./(Mt.^2-1); % Mt center
32
rt = Mt./(Mt.^2-1);     % Mt radius
33
34
35
36
37
38
39
40

% controller structure
K1=[1+0*w 1./(1i*w) 1i*w];
n=numel(p);
K1=K1(:,1:n);
K=K1*p;

% Optimization routine
41
42
prev_obj = -inf; % Used for the stopping criterion
max_iters = 10; % Max number of iterations (heuristic)
43
for iter = 1:max_iters
44
    Lc = P.*K; % Loop transfer funtion
45
46
47
48
49
50
51
    cvx_begin
    cvx_quiet(true)
    variable p(n)
    maximize (p(2)) % Maximize integral gain
    subject to
    K = K1*p;
    L = P.*K;
52
53
54
55
56
57
58
59
60
    if Ms < inf
        real(conj((Lc-cs)./abs(Lc-cs)).*(L-cs)) >= rs; % Sensitivity constraint
    end
    if Mt < inf
        real(conj((Lc - ct)./abs(Lc-ct)).*(L-ct)) >= rt;  % Complementary sensitivity constraint
    end
    if Mks < inf
        abs(K) - Mks.*real(conj(1+Lc)./(abs(1+Lc)).*(1+L)) <= 0; % Noise sensitivity constraint
    end
61
62
63
64
65
    cvx_end
    if n == 2
        p(3) = NaN;
    end
    
66
    % Stopping criterion (heuristic) - change as needed
67
68
69
70
71
72
73
74
75
76
    if (p(2)-prev_obj) < 1e-3*prev_obj
        break;
    end
    prev_obj = p(2);
end

s = zpk('s');
K1 = [1 1/s s];
K1 = K1(1:n);
K = K1*p;