Commit 3aa8682a authored by Adwait Datar's avatar Adwait Datar
Browse files

Gitlab Runner Update from orc-aux-files/master:Add code for for 18_2, 19_1, 19_2, 21_1

parent beabf586
function h2_lmi = lmi_norm_h2(T)
% LMI_NORM_H2 :: Compute the system H2 norm using LMIs
%
% TUHH :: Institut for Control Systems :: Optimal and Robust Control
% Last update: 23.06.2015
[A,B,C,~] = ssdata(T);
[nout,nstate] = size(C);
% Initialize description of LMI system
setlmis([]);
% Specify matrix variables in LMI problem
P = lmivar(1,[nstate 1]); % symmetric, full block nstate x nstate
W = lmivar(1,[nout 1]); % symmetric, full block nout x nout
% Attach identifier to the first LMI M1 < 0
M1 = newlmi();
% First LMI
% [ AP + PA' B ]
% [ ] < 0
% [ B' -I ]
% Specify the term contents of the first LMI
% Define either Mij or Mji, but not both!
lmiterm([M1 1 1 P],A,1,'s'); % left side, M11 = A*P*I + *
lmiterm([M1 1 2 0],B); % left side, M12 = B
lmiterm([M1 2 2 0],-1); % left side, M22 = -I
% Attach identifier to the second LMI M2 < 0
M2 = newlmi();
% Second LMI
% [ W CP ]
% 0 < [ ]
% [ PC' P ]
% Specify the term contents of the second LMI
% Define either Mij or Mji, but not both!
lmiterm([-M2 1 1 W],1,1); % right side, M11 = W
lmiterm([-M2 1 2 P],C,1); % right side, M12 = C*P*I
lmiterm([-M2 2 2 P],1,1); % right side, M22 = P
% Internal description of LMI system
mylmi = getlmis();
% Total number of decision variables in system of LMIs
n = decnbr(mylmi);
% Theorem 18.2 gives a condition to check, whether the H2 norm is smaller
% than v. Thus, the smallest v for which the LMIs in (18.19) hold will be
% equal to the H2 norm. So far, we did not consider trace(W) < v^2. In
% practice, we will minimize trace(W), which should be equal to the
% smallest v^2, for which |T| < v^2 holds. Once v^2 is obtained, the H2
% norm is equal to sqrt(v^2).
% The LMI lab toolbox in Matlab solves problems of the type
% min c'*p, subject to M(p) < 0.
% Therefore, we have to reformulate our objective function such that
% trace(W) = c'*p.
% Initialization of c
c = zeros(n,1);
% Specify cTx objectives for mincx solver
for ind = 1:n
Wi = defcx(mylmi,ind,W);
c(ind) = trace(Wi);
end
% Minimize linear objective under LMI constraints
[cost, ~] = mincx(mylmi,c);
% H2 norm
h2_lmi = sqrt(cost);
function hinf_lmi = lmi_norm_hinf(T)
% LMI_NORM_HINF :: Compute the system Hinf norm using LMIs
%
% TUHH :: Institut for Control Systems :: Optimal and Robust Control
% Last update: 23.06.2015
[A,B,C,D] = ssdata(T);
nstate = size(A,1);
% Initialize description of LMI system
setlmis([]);
% Specify matrix variables in LMI problem
P = lmivar(1,[nstate 1]); % symmetric, full block nstate x nstate
g = lmivar(XXX); % symmetric, scalar 1 x 1
% Attach identifier to the first LMI M1 < 0
M1 = newlmi();
% First LMI
% [ A'P + PA PB C' ]
% [ B'P -gI D' ] < 0
% [ C D -gI ]
% Specify the term contents of the first LMI
% Define either Mij or Mji, but not both!
XXX
% Attach identifier to the second LMI M2 < 0
M2 = newlmi();
% Second LMI
% 0 < P
% Specify the term content of the second LMI
XXX
% Internal description of LMI system
mylmi = getlmis();
% Total number of decision variables in system of LMIs
n = decnbr(mylmi);
% Theorem 18.3 gives a condition to check, whether the Hinf norm is smaller
% than g. Thus, the smallest g for which the LMIs in (18.22) hold will be
% equal to the Hinf norm.
% The LMI lab toolbox in Matlab solves problems of the type
% min c'*p, subject to M(p) < 0.
% Therefore, we have to reformulate our objective function such that
% g = c'*p.
% Initialization of c
c = zeros(n,1);
% Specify cTx objectives for mincx solver
for ind = 1:n
gi = defcx(XXX);
c(ind) = XXX;
end
% Minimize linear objective under LMI constraints
[cost, ~] = mincx(mylmi,c);
% Hinf norm
hinf_lmi = cost;
function [P,h2_norm]=h2_norm_with_cvx(A,B,C,D,tol)
n=size(A,1);
m=size(B,2);
r=size(C,1);
cvx_clear
cvx_begin sdp
variable P(n,n) symmetric
variable h2_norm_squared
cvx_precision high
minimize h2_norm_squared
subject to:
P>=tol*eye(n)
h2_norm_squared>=tol
A*P+P*A'+ B*B'<=-tol*eye(n)
trace(C*P*C')+tol<=h2_norm_squared
cvx_end
status=cvx_status;
h2_norm=sqrt(h2_norm_squared);
end
\ No newline at end of file
function [hinf_norm,P]=hinf_norm_with_cvx(A,B,C,D,tol)
n=size(A,1);
m=size(B,2);
r=size(C,1);
cvx_begin sdp
variable P(n,n) symmetric
variable gamma_hinf
cvx_precision high
minimize gamma_hinf
F=[A'*P + P*A, P*B, C';
B'*P, -gamma_hinf*eye(m), D';
C, D, -gamma_hinf*eye(r)];
subject to:
gamma_hinf>=tol
P>=tol*eye(n);
F<=-tol*eye(size(F,1));
cvx_end
status=cvx_status;
hinf_norm=gamma_hinf;
end
\ No newline at end of file
% ORC 18.2 :: H2 and Hinf norm evaluation using LMIs
%
% TUHH :: Institut for Control Systems :: Optimal and Robust Control
% Last update: 16.06.2020
clear
clc
% Results from Problem 15.2
addpath(genpath('.\'))
load orc15_design2 CL2 Gyr2
T = Gyr2;
%% H2 norm
h2_lmi = lmi_norm_h2(T) % LMIs
h2_norm = norm(T, 2) % norm function
tol=1e-6;
[P,h2_norm_cvx]=h2_norm_with_cvx(Gyr2.A,Gyr2.B,Gyr2.C,Gyr2.D,tol); % Using cvx
%% Hinf norm
hinf_lmi = lmi_norm_hinf_(T) % LMIs
hinf_norm = norm(T, inf) % norm function
[hinf_norm_cvx,P]=hinf_norm_with_cvx(Gyr2.A,Gyr2.B,Gyr2.C,Gyr2.D,tol); % using cvx
function model = aircraft_model()
% ORC :: Aircraft model
%
% TUHH :: Institut for Control Systems :: Optimal and Robust Control
% Last update: 12.05.2009
% x1 = relative attitude
% x2 = forward speed
% x3 = pitch angle
% x4 = pitch rate
% x5 = vertical speed
A = [0 0 1.1320 0 -1;
0 -0.0538 -0.1712 0 0.0705;
0 0 0 1 0;
0 0.0485 0 -0.8556 -1.013;
0 -0.2909 0 1.0532 -0.6859];
% u1 = spoiler angle
% u2 = forward acceleration
% u3 = elevator angle
B = [0 0 0;
-0.12 1 0;
0 0 0;
4.419 0 -1.665;
1.575 0 -0.0732];
% y1=x1 = relative attitude
% y2=x2 = forward speed
% y3=x3 = pitch angle
C = [eye(3) zeros(3,2)];
D = zeros(3,3);
model = ss(A,B,C,D);
% This file collects functions to synthesize H2 output-feedback synthesis
% problem
function [K_ss,h2_opt] = cvx_H2OFB(GSYS,ncont,nmeas,tol,regularize,relax)
% This function synthesizes a controller that minimizes the closed-loop
% H2 norm
% Follows Notation from: LMI methods in control by Scherer and Weiland
% Still to be implemented:
% 1. Discrete-time systems
% 2. LPV systems: Continuous and Discrete-time systems
% 3. Eliminate the variables via Dualization Lemma and Elimination Lemma
G=get_model_mat(GSYS,ncont,nmeas);
% STEP 1: First solve for minimizing the H2 norm to get an estimate of
% achievable H2 norm
[v,h2_opt,~,~]=H2_optimization_problem_cvx(G,tol,'step1');
if regularize==1
% STEP 2: Relax the optimal norm and add this as constraint.
% Minimize the regularization parameter alpha.
h2_opt=relax*h2_opt;
[~,~,alpha_opt,~]=H2_optimization_problem_cvx(G,tol,'step2',h2_opt);
% STEP 3: Relax the optimal alpha and add this as constraint.
% Maximize the conditioning parameter beta.
alpha_opt=relax*alpha_opt;
[v,~,~,beta_opt]=H2_optimization_problem_cvx(G,tol,'step3',h2_opt,alpha_opt);
end
K_mat=get_control_matrices(v,G);
n=size(G.A,1);
K_ss=ss(K_mat(1:n,1:n),K_mat(1:n,n+1:n+nmeas),K_mat(n+1:n+ncont,1:n),K_mat(n+1:n+ncont,n+1:n+nmeas));
end
function [v,h2_opt,alpha_opt,beta_opt]=H2_optimization_problem_cvx(G,tol,method,h2_opt,alpha_opt,beta_opt)
% This function solves the H2 optimal control problem in the substituted
% variables v and returns the optimal v
[nx,nw,nz,ncont,nmeas]=get_sizes(G);
cvx_clear
cvx_begin sdp
cvx_precision best
variable X(nx,nx) symmetric
variable Y(nx,nx) symmetric
variable Z(nz,nz) symmetric
variable K(nx,nx)
variable L(nx,nmeas)
variable M(ncont,nx)
variable N(ncont,nmeas)
variable gamma_h2
variable alpha_reg
variable beta_cond
% Define new functions of variables for convenience
X_v=XXXXXX;
A_v=XXXXXX;
B_v=XXXXXX;
C_v=XXXXXX;
D_v=XXXXXX;
KLMN=[K,L;M,N];
% Define LMIs for usual H2 synthesis
LMI_1=[ A_v+A_v', B_v;...
B_v', -gamma_h2*eye(nw)];
LMI_2=[ X_v, C_v';
C_v, Z];
LMI_3=D_v;
LMI_4=trace(Z)-gamma_h2;
% Define Regularization LMI
Reg_LMI_1=X-alpha_reg*eye(nx);
Reg_LMI_2=Y-alpha_reg*eye(nx);
Reg_LMI_3=[ alpha_reg*eye(nx+ncont), KLMN;...
KLMN', alpha_reg*eye(nx+ncont)];
Reg_LMI_4=[ Y, beta_cond*eye(nx);...
beta_cond*eye(nx), X];
switch lower(method)
case {'step1'}
minimize gamma_h2
subject to:
LMI_1<=-tol*eye(2*nx+nw)
LMI_2>=tol*eye(2*nx+nz)
LMI_3==zeros(nz,nw)
LMI_4<=-tol
case {'step2'}
minimize alpha_reg
subject to:
LMI_1<=-tol*eye(2*nx+nw)
LMI_2>=tol*eye(2*nx+nz)
LMI_3==zeros(nz,nw)
LMI_4<=-tol
Reg_LMI_1<=-tol*eye(nx)
Reg_LMI_2<=-tol*eye(nx)
Reg_LMI_3>=tol*eye(2*(nx+ncont))
gamma_h2==h2_opt
case {'step3'}
minimize -1*beta_cond
subject to:
LMI_1<=-tol*eye(2*nx+nw)
LMI_2>=tol*eye(2*nx+nz)
LMI_3==0%zeros(nz,nw)
LMI_4<=-tol
Reg_LMI_1<=-tol*eye(nx)
Reg_LMI_2<=-tol*eye(nx)
Reg_LMI_3>=tol*eye(2*(nx+ncont))
Reg_LMI_4>=tol*eye(2*nx)
gamma_h2==h2_opt
alpha_reg==alpha_opt
end
cvx_end
h2_opt=gamma_h2;
alpha_opt=alpha_reg;
beta_opt=beta_cond;
v=struct;
v.X=X; v.Y=Y; v.K=K; v.L=L; v.M=M; v.N=N;
end
function [K]=get_control_matrices(v,G)
% This function gives back the controller variables and the Lyapunov atrix from the
% substitute variables v
n=size((v.X),1);
[U,S,V]=svd(eye(n)-(v.X)*(v.Y));
U=U*sqrt(S);
V=(sqrt(S)*V')';
K=XXXXXXXXXXXXXX;
end
function [nx,nw,nz,ncont,nmeas]=get_sizes(G)
% Get sizes of signals
nx=size(G.A,1);
nw=size(G.B1,2);
ncont=size(G.B,2);
nmeas=size(G.C,1);
nz=size(G.C1,1);
end
function G=get_model_mat(G_genp,ncont,nmeas)
% This function returns model matrices as a struct
[A_gp,B_gp,C_gp,D_gp]=ssdata(G_genp);
nx=size(A_gp,1);
nw=size(B_gp,2)-ncont;
nz=size(C_gp,1)-nmeas;
G=struct;
G.A=A_gp;
G.B1=B_gp(:,1:nw);
G.B=B_gp(:,nw+1:end);
G.C1=C_gp(1:nz,:);
G.C=C_gp(nz+1:end,:);
G.D1=D_gp(1:nz,1:nw);
G.E=D_gp(1:nz,nw+1:end);
G.F=D_gp(nz+1:end,1:nw);
end
function [K,h2norm] = lmi_h2syn( G, nmeas, ncont )
% LMI_H2SYN Design H2 controllers using LMIs
%
% [K,h2norm] = lmi_h2syn( GSYS, nmeas, ncont )
% GSYS - generalized plant
% nmeas - number of measured outputs
% ncont - number of controlled inputs
% K - obtained controller
% h2norm - H2 norm with the obtained controller
% TUHH :: Institut for Control Systems :: Optimal and Robust Control
% Last update: 28.06.2016
%% Extract the matrices
[A,B,C,D] = ssdata(G);
n = size(A,1);
[no,ni] = size(D);
nw = XXX;
nz = XXX;
%% Fragment the matrices
Bw = B(:,1:nw);
Bu = B(:,nw+1:end);
Cz = C(1:nz,:);
Cv = C(nz+1:end,:);
Dzu = D(1:nz, nw+1:end);
Dvw = D(nz+1:end, 1:nw);
%% Construct the LMIs
% Initialisation
setlmis([]);
% Matrix variables
X_ = lmivar(XXX);
Y_ = lmivar(XXX);
W_ = lmivar(XXX);
Ak_ = lmivar(XXX); % Ak-tilde
Bk_ = lmivar(XXX);
Ck_ = lmivar(XXX);
% % Set the Dk matrix to 0 to satisfy the requirement
% % Dc = Dzw + Dzu*Dk*Dvw = 0
% Dk_ = lmivar(2,[ncont nmeas]);
% First LMI
M0 = newlmi();
lmiterm([-M0 1 1 Y_ ],1, 1);
lmiterm([-M0 1 2 0 ],XXX);
lmiterm([-M0 2 2 X_ ],1, 1);
% Second LMI
M1 = newlmi();
lmiterm([M1 1 1 Y_ ],A, 1,'s');
lmiterm([M1 1 1 Ck_],Bu,1,'s');
lmiterm([M1 1 2 -Ak_],1, 1); % Ak-tilde transposed
lmiterm([M1 1 2 0 ],A);
% lmiterm([M1 1 2 Dk_],Bu,Cv); % DK
lmiterm([M1 2 2 X_ ],1, A,'s');
lmiterm([M1 2 2 Bk_],1,Cv,'s');
lmiterm(XXX);
lmiterm(XXX);
lmiterm([M1 3 2 0 ],Cz);
% lmiterm([M1 3 2 Dk_],Dzu,Cv); % DK
lmiterm([M1 3 3 0 ],XXX);
% Third LMI
M2 = newlmi();
lmiterm([-M2 1 1 Y_ ],1,1);
lmiterm([-M2 1 2 0 ],eye(n));
lmiterm([-M2 1 3 0 ],Bw);
% lmiterm([-M2 1 3 Dk_],Bu,Dvw); % DK
lmiterm([-M2 2 2 X_ ],1,1);
lmiterm([-M2 2 3 X_ ],1,Bw);
lmiterm([-M2 2 3 Bk_],1,Dvw);
lmiterm([-M2 3 3 W_ ],1,1);
% System of LMIs
LMIs = getlmis ;
% Objective function
nvar = decnbr(LMIs);
c = zeros(nvar,1);
for i = 1:nvar
Wi = defcx(LMIs,i,W_);
c(i) = trace(Wi);
end
% Optimisation
[cost,xopt] = mincx(LMIs,c);
if isempty(cost)
K = ss(zeros(ncont,nmeas));
h2norm = inf;
display('No feasible solution found');
return;
end
% H2 norm
h2norm = sqrt(cost);
% Optimal values of matrix variables
X = dec2mat(LMIs,xopt,X_);
Y = dec2mat(LMIs,xopt,Y_);
% W = dec2mat(LMIs,xopt,W_);
Ak = dec2mat(LMIs,xopt,Ak_);
Bk = dec2mat(LMIs,xopt,Bk_);
Ck = dec2mat(LMIs,xopt,Ck_);
% Dk = dec2mat(LMIs,xopt,Dk_); % DK
% Singular Value Decomposition
[U,S,V] = svd(XXX);
R = U*sqrt(S);
S = (sqrt(S)*V')';
% Inverse transformations
% Dk_fin = Dk;
Dk_fin = zeros(XXX); % DK = 0
Ck_fin = (Ck - Dk_fin*Cv*Y)*inv(S');
Bk_fin = inv(R)*(Bk-X*Bu*Dk_fin);
Ak_fin = inv(R)*(Ak - R*Bk_fin*Cv*Y - X*Bu*Ck_fin*S' - X*(A+Bu*Dk_fin*Cv)*Y )*inv(S');
% Controller
K = ss(Ak_fin, Bk_fin, Ck_fin, Dk_fin);
% ORC 19.1 :: LQG design via LMIs
%
% TUHH :: Institut for Control Systems :: Optimal and Robust Control
% Last update: 14.07.2020
clear
clc
%% Load the model
Gairc = aircraft_model();
[A,B,C,D] = ssdata(Gairc);
[nmeas, ncont] = size(D); % number of measured and controlled signals
nstates = size(A,1);