SOFOpt.m

function optim_out = SOFOpt(A, B, C, Q, R, X0, structure_or_opt, opt)
%SOFOPT Static output feedback optimization for continuous-time systems.
%
%   optim_out = SOFOpt(A,B,C,Q,R,X0) solves a continuous-time static output
%   feedback LQR problem using the default optimization options.
%
%   The plant is
%
%       dx/dt = A*x + B*u
%       y    = C*x
%       u    = -Ky*y = -Kx*x
%
%   where Kx = Ky*C. The objective is to minimize
%
%       J(X0) = integral_0^inf (x(t)'*Q*x(t) + u(t)'*R*u(t)) dt
%
%   for the initial condition X0.
%
%   optim_out = SOFOpt(A,B,C,Q,R,X0,structure) imposes a sparsity structure
%   on Ky. The matrix structure must have the same size as Ky. Entries equal
%   to zero are fixed to zero, while nonzero entries are optimized.
%
%   optim_out = SOFOpt(A,B,C,Q,R,X0,opt) uses the optimization options
%   specified by opt. See help SOFOptimoptions for details.
%
%   optim_out = SOFOpt(A,B,C,Q,R,X0,structure,opt) uses both a prescribed
%   sparsity structure and user-defined optimization options.
%
%   The returned structure optim_out contains:
%
%       Ky           Optimal static output feedback gain.
%       Kx           Equivalent state feedback gain, Kx = Ky*C.
%       LQR_cost     Cost J(X0) obtained with full-state LQR.
%       Output_cost  Cost J(X0) obtained with static output feedback.
%       Result       Optimization result string returned by NLopt.
%
%   See also SOFOptimoptions.

if nargin < 6
    error("SOFOpt(): At least six arguments are required");
end

if nargin == 6
    structure = ones(size(B, 2), size(C, 1), 'logical');
    opt = SOFOptimoptions;
elseif nargin == 7
    if isstruct(structure_or_opt)
        opt = structure_or_opt;
        structure = ones(size(B, 2), size(C, 1), 'logical');
    else
        structure = structure_or_opt;
        opt = SOFOptimoptions;
    end
else
    structure = structure_or_opt;
end

this_dir = fileparts(mfilename('fullpath'));

if ispc
    exec_path = fullfile(this_dir, "SOFOpt", "out", "build", "SOFOpt-MSVC", "bin", "solver_exec.exe");
elseif isunix
    exec_path = fullfile(this_dir, "SOFOpt", "out", "build", "SOFOpt-Linux", "bin", "solver_exec");
else
    error("Unsupported platform");
end

setenv("OUTPUTFEEDBACK_SOLVER_EXEC", exec_path);

[Kx, Ky, init_cost, optim_cost, result] = SOFOpt_exec(A, B, C, Q, R, X0, ...
    structure, opt.use_P_precond, opt.rho_alpha, opt.beta, opt.r, opt.c);

spectral_abs = max(real(eig(A - B*Ky*C)));

if ~strcmp(result, 'XTOL_REACHED') || spectral_abs >= 0
    warning("SOFOptim(): Failed to converge to a feasible solution. Try adjusting the problem formulation or solver parameters. " + ...
        "The returned controller may be unstable or may violate the specified stability margins.");
end

optim_out.Ky = Ky;
optim_out.Kx = Kx;
optim_out.LQR_cost = init_cost;
optim_out.Output_cost = optim_cost;
optim_out.Result = result;

end