%System_parameters
m = 1800; % mass in kg
Iz= 2500; % Vehicle total inertia about the yaw axis in kg m2
lf= 1.03; % Distance of the front axle from CG in m
lr= 1.49; % Distance of the rear axle from CG in m
C_f= 40; % Nominal cornering stiffness of front tyres in KN/rad
C_r= 40; % Nominal cornering stiffness of rear tyres in KN/rad
D_l = 10; %look ahead distance in m
%for varying parameter vector p = (p1,...,ps)
v_x_min= 5;
v_x_max= 35;
%sys {v_x_min, 1/v_x_max}
A{1} = [-(C_r+C_f)./(m*v_x_max) ((lr*C_r)-(lf*C_f))./(m*v_x_max)-v_x_min 0 0; ((lr*C_r)-(lf*C_f))./(Iz*v_x_max) -((lr^2*C_r)+(lf^2*C_f))./(Iz*v_x_max) 0 0; -1 -D_l 0 v_x_min; 0 -1 0 0];
B{1} = [C_f./m; (lf*C_f)./Iz; 0; 0];
C{1} = [0 1 0 0; 0 0 1 0; 0 0 0 1];
E{1}=[0; 0; 0; v_x_min];
D{1} = zeros(3,1);
s1= ltisys(A{1},B{1},C{1},D{1});
%Sys {v_x_min, 1/v_x_min}
A{2} = [-(C_r+C_f)./(m*v_x_min) ((lr*C_r)-(lf*C_f))./(m*v_x_min)-v_x_min 0 0; ((lr*C_r)-(lf*C_f))./(Iz*v_x_min) -((lr^2*C_r)+(lf^2*C_f))./(Iz*v_x_min) 0 0; -1 -D_l 0 v_x_min; 0 -1 0 0];
B{2} = [C_f./m; (lf*C_f)./Iz; 0; 0];
C{2} = [0 1 0 0; 0 0 1 0; 0 0 0 1];
E{2}=[0; 0; 0; v_x_min];
D{2} = zeros(3,1);
s2= ltisys(A{2},B{2},C{2},D{2});
% Sys{v_x_max, 1/v_x_min}
A{3} = [-(C_r+C_f)./(m*v_x_min) ((lr*C_r)-(lf*C_f))./(m*v_x_min)-v_x_max 0 0; ((lr*C_r)-(lf*C_f))./(Iz*v_x_min) -((lr^2*C_r)+(lf^2*C_f))./(Iz*v_x_min) 0 0; -1 -D_l 0 v_x_max; 0 -1 0 0];
B{3} = [C_f./m; (lf*C_f)./Iz; 0; 0];
C{3} = [0 1 0 0; 0 0 1 0; 0 0 0 1];
E{3}=[0; 0; 0; v_x_max];
D{3} = zeros(3,1);
s3= ltisys(A{3},B{3},C{3},D{3});
% Sys {v_x_max, 1/v_x_max}
A{4} = [(C_r+C_f)./(m*v_x_max) ((lr*C_r)-(lf*C_f))./(m*v_x_max) - v_x_max 0 0; ((lr*C_r)-(lf*C_f))./(Iz*v_x_max) -((lr^2*C_r)+(lf^2*C_f))./(Iz*v_x_max) 0 0; -1 -D_l 0 v_x_max; 0 -1 0 0];
B{4} = [C_f./m; (lf*C_f)./Iz; 0; 0];
C{4} = [0 1 0 0; 0 0 1 0; 0 0 0 1];
E{4}=[0; 0; 0; v_x_max];
D{4} = zeros(3,1)
s4= ltisys(A{4},B{4},C{4},D{4});
pols = psys([s1,s2,s3,s4]);
[tmin,P] = quadstab(pols)
%% LMI Solution - Prepare Yalmip
yalmip('clear');
LMIs = [];
gamma2=sdpvar(1);
Z = sdpvar(4,4,'symmetric');
N = 4;
r = 40;
q = 40;
Z >= 0;
for k = 1:N % N is number of vertices
W{k} = sdpvar(1,4,'full');
%% Condition for Quadratic Stability
LMIs = [LMIs, A{k}*Z + Z*A{k}'+ B{k}*W{k} + W{k}'*B{k}'<= 0];
%% Condition for H_∞ Performance
LMIs = [LMIs, [A{k}*Z+Z'*A{k}'+B{k}*W{k}+W{k}'*B{k}' E{k} Z'*C{k}'; E{k}' -eye(1) D{k}'; C{k}*Z D{k} -gamma2*eye(3)] <= 0];
%% Condition for LMI Reion
LMIs = [LMIs, [-r*Z q*Z+A{k}*Z+B{k}*W{k}; q*Z+Z'*A{k}'+W{k}'*B{k}' -r*Z] <= 0];
end
%options = sdpsettings('solver','gurobi');
options = sdpsettings('solver','mosek');
solution1 = optimize(LMIs,Z,options)
solution2 = optimize(LMIs,gamma2,options)
%% Controller Gain at Individual Vertices
K1 = value(W{1})*inv(value(Z))
K2 = value(W{2})*inv(value(Z))
K3 = value(W{3})*inv(value(Z))
K4 = value(W{4})*inv(value(Z))