Commit 58e6827e authored by Kristian Soltesz's avatar Kristian Soltesz
Browse files

Code cleanup

parent 71a8178e
function [K,Pd,Q,opts] = optIAE(P,Ms,Mt,Mks,opts)
% Computes approximation of IAE-optimal LTI controller subject to
% constraints on the magnitude of the sensitivity, complementary
% sensitivity and noise sensitivity functions.
% Computes a finite-order, discrete-time approximation of the IAE-optimal
% LTI controller subject to constraints on the magnitude of the
% sensitivity, complementary sensitivity and noise sensitivity functions.
%
% K - Approximation of the optimal LTI controller
% K - Discrete-time approximation of the optimal LTI controller
%
% P - LTI process model.
% Ms - Maximum allowed sensitivity.
......@@ -23,17 +23,17 @@ T = opts.T;
K0 = opts.K0;
Nq = opts.Nq;
% Sample the plant
% Sample the plant using first-order hold
Pd = balreal(delay2z(c2d(balreal(P),h,'foh')));
[AP,BP,CP,DP] = ssdata(Pd);
% Time horizon
Tsteps = floor(T/h); % normalize wrt samlpling period
Tsteps = floor(T/h); % normalize wrt sampling period
% Optimize using CVX
cvx_clear
cvx_begin
cvx_solver mosek
cvx_solver mosek % sedumi also works, but is slower
cvx_precision default
cvx_quiet(true)
variable q(Nq)
......@@ -46,17 +46,17 @@ if max(abs(pole(Q0d))) > 1
end
[AQ0,BQ0,CQ0,DQ0] = ssdata(Q0d);
% Additional Q parameterization by finite pulse response
% Additional Q parametrization by finite pulse response
AQ1 = diag(ones(Nq-2,1),1); % Q = K/(1+PK)
BQ1 = q(2:Nq,1); % The other Nq-1 values of pulse response
CQ1 = [1 zeros(1,Nq-2)];
DQ1 = q(1); % The first (k=0) value of pulse response
[AQ,BQ,CQ,DQ] = parallell(AQ1,BQ1,CQ1,DQ1,AQ0,BQ0,CQ0,0*DQ0);
[AQ,BQ,CQ,DQ] = parallel_ss(AQ1,BQ1,CQ1,DQ1,AQ0,BQ0,CQ0,DQ0); % Q = Q0 + Q1
% Calculate closed-loop state-space systems
[AT,BT,CT,DT] = series(AQ,BQ,CQ,DQ,AP,BP,CP,DP); % = T = P*Q
[AS,BS,CS,DS] = parallell(0,0,0,1,AT,-BT,CT,-DT); % = S = 1-T
[APS,BPS,CPS,DPS] = series(AS,BS,CS,DS,AP,BP,CP,DP); % = P*S
[AT,BT,CT,DT] = series_ss(AQ,BQ,CQ,DQ,AP,BP,CP,DP); % T = P * Q
[AS,BS,CS,DS] = parallel_ss(AT,-BT,CT,-DT,0,0,0,1); % S = 1 - T
[APS,BPS,CPS,DPS] = series_ss(AS,BS,CS,DS,AP,BP,CP,DP); % P * S
if max(abs(eig(APS))) >= 1
error('P*S not stable')
end
......@@ -73,7 +73,6 @@ if Mks < Inf
end
% Calculate pulse response d -> y
clear y
y(1) = DPS;
for k = 2:Tsteps
y(k) = CPS * APS^(k-2) * BPS;
......@@ -84,7 +83,7 @@ minimize norm(y,1) % IAE
subject to:
% Constraint on S = 1/(1+PK)
% H-inf constraint on S = 1/(1+PK)
XS = [PS AS*PS BS zeros(nS,1);
PS'*AS' PS zeros(nS,1) PS*CS';
BS' zeros(1,nS) 1 DS';
......@@ -92,7 +91,7 @@ XS = [PS AS*PS BS zeros(nS,1);
XS == semidefinite(2*nS+2)
% Constraint on T = PK/(1+PK)
% H-inf constraint on T = PK/(1+PK)
XT = [PT AT*PT BT zeros(nT,1);
PT'*AT' PT zeros(nT,1) PT*CT';
BT' zeros(1,nT) 1 DT';
......@@ -100,7 +99,7 @@ XT = [PT AT*PT BT zeros(nT,1);
XT == semidefinite(2*nT+2)
% Constraint on Q = K/(1+PK)
% H-inf constraint on Q = K/(1+PK)
if Mks < Inf
XQ = [PQ AQ*PQ BQ zeros(nQ,1);
PQ'*AQ' PQ zeros(nQ,1) PQ*CQ';
......@@ -117,8 +116,7 @@ CQ/(eye(size(AQ))-AQ)*BQ+DQ == invdcg
cvx_end
% Check for success
if strcmp(cvx_status,'Failed') || strcmp(cvx_status,...
'Inaccurate/Infeasible')
if strcmp(cvx_status,'Failed') || strcmp(cvx_status,'Inaccurate/Infeasible')
error('Optimization failed')
end
......@@ -127,39 +125,20 @@ Q = ss(AQ,BQ,CQ,DQ,h);
K = feedback(Q,-Pd);
end
function [a,b,c,d] = parallell(a1,b1,c1,d1,a2,b2,c2,d2)
% Parallell interconnection of ss systems
[msg,a1,b1,c1,d1]=abcdchk(a1,b1,c1,d1); error(msg);
[msg,a2,b2,c2,d2]=abcdchk(a2,b2,c2,d2); error(msg);
[ny1,nu1] = size(d1);
[ny2,nu2] = size(d2);
% State space systems w/o selection vectors
inputs1 = [1:nu1]; outputs1 = [1:ny1];
inputs2 = [1:nu2]+nu1; outputs2 = [1:ny2]+ny1;
% Check sizes
if (length(inputs1)~=length(inputs2))
error('Input sizes don''t match.')
elseif (length(outputs1)~=length(outputs2))
error('Output sizes don''t match.')
end
% Parallel Connection
function [a,b,c,d] = parallel_ss(a1,b1,c1,d1,a2,b2,c2,d2)
% Helper function to construct the parallel connection
% ss(a,b,c,d) = ss(a1,b1,c1,d1) + ss(a2,b2,c2,d2)
a = blkdiag(a1,a2);
b = blkdiag(b1,b2);
c = blkdiag(c1,c2);
d = blkdiag(d1,d2);
% Connect inputs
if ~isempty(b), b(:,inputs1)=b(:,inputs1)+b(:,inputs2); end
if ~isempty(d), d(:,inputs1)=d(:,inputs1)+d(:,inputs2); end
% Connect outputs
if ~isempty(c), c(outputs1,:)=c(outputs1,:)+c(outputs2,:); end
if ~isempty(d), d(outputs1,:)=d(outputs1,:)+d(outputs2,:); end
b = [b1; b2];
c = [c1 c2];
d = d1 + d2;
end
% Delete redundant inputs and outputs
b(:,inputs2) = []; d(:,inputs2) = [];
c(outputs2,:) = []; d(outputs2,:) = [];
function [a,b,c,d] = series_ss(a1,b1,c1,d1,a2,b2,c2,d2)
% Helper function to construct the series connection
% ss(a,b,c,d) = ss(a2,b2,c2,d2) * ss(a1,b1,c1,d1)
a = [a1 zeros(size(a1,1),size(a2,2)); b2*c1 a2];
b = [b1; b2*d1];
c = [d2*c1 c2];
d = d2 * d1;
end
......@@ -25,8 +25,8 @@ end
% Extract options
w = opts.w;
p = opts.p;
maxIter = opts.maxIter; % 100
stopTol = opts.stopTol; % 1e-3
maxIter = opts.maxIter;
stopTol = opts.stopTol;
% Frequency response
P = squeeze(freqresp(balreal(P),w));
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment