clear
clc
close all
yalmip('clear')
A = [-1 2;-3 -4];
B = [1;1];
m = [-2.3123;37.6877]; % feedforward components
% x = [-5;-5] % equilibrium point
% dot{x} = A * x + B * u;
% u = -Kx + m
% dot{x} = (A - BK) * x + m;
% V_A = x^T * P_A * X + 2 q_A^T x + r_A
P_A = sdpvar(2,2);
q_A = sdpvar(2,1);
r_A = sdpvar(1,1);
lambda = sdpvar(1,1); % s - procedure to force this only hold for the region y < - x - 1
H_A = [1;1];
g_A = [-1];
K = sdpvar(1,2);
lAB = [-1;0];
FAB = [-1;1];
lAC = [1;-2];
FAC = [-1,1];
lBC = [1;-2];
FBC = [2;2];
constraints = [...
[P_A, q_A + H_A * lambda;
q_A.' + lambda.' * H_A.', r_A - 2 * g_A .' * lambda] >= 0,... % This is the positivity constraints
]
constraints = [constraints,...
[(A-B*K).' * P_A + P_A * (A-B*K), P_A * m + (A-B*K).'*q_A;
m.'*P_A + q_A.'*(A-B*K) , 2*q_A.'*m] <= zeros(3)... % decreasing
]
options = sdpsettings('solver','bmibnb');
optimize(constraints,trace(P_A),options);
sol_P = value(P_A) %
sol_K = value(K) % gain