% ### LMI H2-norm bound analysis ###
%
% SPDX-FileCopyrightText: 2023 Deutsches Elektronen-Synchrotron DESY, MSK, LbSync <lbsync-expert@desy.de>
% SPDX-License-Identifier: GPL-3.0-or-later
%
% Author: Maximilian Schütte <maximilian.schuette@desy.de>

function [gamma, info] = h2lmi(P, opts)
%H2LMI Optimizes a non-conservative H2-norm upper bound using LMI methods.
%   Implements LMI analysis methods from Schwerer & Weiland "LMIs in Control".
%
% Author: Maximilian Schütte <maximilian.schuette@desy.de>

arguments
    % The plant to be analyzed.
    P lti

    % Force definitness constraints to satisfy this margin.
    opts.tolerance (1, 1) double {mustBeNonnegative} = 1e-9

    % Hard upper bound on H2-norm optimization variable.
    opts.hard_bound (1, 1) double {mustBePositive} = inf

    % YALMIP options.
    opts.sdp_options (1, 1) struct = sdpsettings()
end

mustBeNonnegative(opts.tolerance);
tol = opts.tolerance;

info = struct();

P = ss(P);

nx = size(P.A, 1);
nw = size(P.B, 2);
nz = size(P.C, 1);

A = P.A;
B = P.B;
C = P.C;
D = P.D;

if any(D)
    warning("System has non-zero feedthrough and hence unbounded H2-norm.")
    gamma = inf;
    return;
end

X = sdpvar(nx);
Y = sdpvar(nx);
%Z = sdpvar(nz);

gamma_sq = sdpvar();


cnst = [trace(C*X*C') <= gamma_sq;
         A*X + X*A' + B*B' <= -tol * eye(nx)];

if isfinite(opts.hard_bound)
    cnst = [cnst;
            gamma_sq <= opts.hard_bound];
end

info.ctrl.sol_diag = optimize(cnst, gamma_sq, opts.sdp_options);
info.ctrl.constraints = cnst;
info.ctrl.X = value(X);
info.ctrl.gamma = sqrt(value(gamma_sq));

cnst = [trace(B'*Y*B) <= gamma_sq;
         A'*Y + Y*A + C'*C <= -tol * eye(nx)];

if isfinite(opts.hard_bound)
    cnst = [cnst;
            gamma_sq <= opts.hard_bound];
end

info.obsv.sol_diag = optimize(cnst, gamma_sq, opts.sdp_options);
info.obsv.constraints = cnst;
info.obsv.Y = value(Y);
info.obsv.gamma = sqrt(value(gamma_sq));

gamma = min(info.ctrl.gamma, info.obsv.gamma);

end