clear all
close all
clc
%% General Parameters definition
cm = 0.03;
J1 = 0.001;
J2 = 0.004;
Rr = 0.5;
Rs = 85;
Lr = 8.9e-3;
Ls = 50;
Bm = 0.009;
wref = 120;
Iref = 0.1;
pref = 15;
usref = Rs*pref;
urref = Rr*Iref+cm*pref*wref;
Qin = diag([4,40,40]);
Qout = diag([2.5e-5,2.5e-5,0.0025]);
Xin = Qin^-0.5;
%% parameters of Sys. 1
A1 = [-Bm/J1 cm/J1*pref cm/J1*Iref;
-cm/Lr*pref -Rr/Lr -cm/Lr*wref;
0 0 -Rs];
B1 = [0 0;
0 1/Lr;
1 0];
C1 = eye(3);
M1 = 15*cm*[0 1/J1 0;
1/Lr 0 0;
0 0 0];
D1 = 0.1*zeros(3);
Ep1 = zeros(3,1);
Ea1 = zeros(3);
Eb1 = zeros(3,2);
%% parameters of Sys. 2
A2 = [-Bm/J2 cm/J2*pref cm/J2*Iref;
-cm/Lr*pref -Rr/Lr -cm/Lr*wref;
0 0 -Rs];
B2 = [0 0;
0 1/Lr;
1 0];
C2 = eye(3);
M2 = 15*cm*[0 1/J2 0;
1/Lr 0 0;
0 0 0];
D2 = 0.1*zeros(3);
Ep2 = zeros(3,1);
Ea2 = zeros(3);
Eb2 = zeros(3,2);
%% desired parameters
m = 2;
n = 2;
tmin = 0.5;
tmax = 1;
rhod = 0.5;
umax = 100;
%% dummy parameters
myeps = 0;
myinf = 1e5;
mypi = 2;
%% general decision parameters
r = sdpvar(1);
rp = sdpvar(1);
delta = 1;
deltap = sdpvar(1);
deltaz = sdpvar(1);
ron = sdpvar(1);
roff = sdpvar(1);
mu = sdpvar(1);
kesi1 = sdpvar(1);
kesi2 = sdpvar(1);
t11 = sdpvar(1);
t21 = sdpvar(1);
t31 = sdpvar(1);
t41 = sdpvar(1);
t51 = sdpvar(1);
t61 = sdpvar(1);
t12 = sdpvar(1);
t22 = sdpvar(1);
t32 = sdpvar(1);
t42 = sdpvar(1);
t52 = sdpvar(1);
t62 = sdpvar(1);
ga1 = sdpvar(1);
gb1 = sdpvar(1);
gp1 = sdpvar(1);
ga2 = sdpvar(1);
gb2 = sdpvar(1);
gp2 = sdpvar(1);
L = sdpvar(3);
W1 = sdpvar(2,3,'full');
W2 = sdpvar(2,3,'full');
%% constraints regards to sys 1.
F11 = [-t11*r+t21*delta Ep1';
Ep1 gp1*eye(3)]>=0;
psi1 = L*A1'+A1*L+W1'*B1'+B1*W1+kesi1*eye(3)+(gp1+ga1+gb1)*(D1'*D1);
lam1 = ron;
F21 = [t11*L-t21*L-psi1-lam1*L L*M1' L*Ea1' W1'*Eb1';
M1*L kesi1*eye(3) zeros(3) zeros(3);
Ea1*L zeros(3) ga1*eye(3) zeros(3);
Eb1*W1 zeros(3) zeros(3) gb1*eye(3)]>=0;
F31 = -t31*r+t41*delta>=0;
F41 = [t31*L-t41*L+mu*L L*C1';
C1*L L]>=0;
F51 = deltap-t51*delta>=0;
F61 = [t51*L L*C1';
C1*L L]>=0;
F71 = umax^2-t61*r>=0;
F81 = [t61*L W1';
W1 eye(2)]>=0;
%% constraints regards to sys 2.
F12 = [-t12*r+t22*delta Ep2';
Ep2 gp2*eye(3)]>=0;
psi2 = L*A2'+A2*L+W2'*B2'+B2*W2+kesi2*eye(3)+(gp2+ga2+gb2)*(D2'*D2);
lam2 = ron;
F22 = [t12*L-t22*L-psi2-lam2*L L*M2' L*Ea2' W2'*Eb2';
M2*L kesi2*eye(3) zeros(3) zeros(3);
Ea2*L zeros(3) ga2*eye(3) zeros(3);
Eb2*W2 zeros(3) zeros(3) gb2*eye(3)]>=0;
F32 = -t32*r+t42*delta>=0;
F42 = [t32*L-t42*L+mu*L L*C2';
C2*L L]>=0;
F52 = deltap-t52*delta>=0;
F62 = [t52*L L*C2';
C2*L L]>=0;
F72 = umax^2-t62*r>=0;
F82 = [t62*L W2';
W2 eye(2)]>=0;
%% General Constraints
F10 = L<=inv(Qin);
F20 = [L Xin';
Xin r*eye(3)]>=0;
F30 = inv(Qout)-r*L>=0;
F40 = (ron+roff)*tmin/tmax-(mu-1)/tmin-roff>=0;
F50 = mypi*deltap <= deltaz <=r;
F60 = mypi*rp <= r;
F70 = (m-n)*(mu-1)+roff*(tmax-n*tmin)<=log(mypi);
%% Constructing the optimization problem
F = [...
% F10,...
% F20,...
% F30,...
F40, F50, F60, F70,...
F11, F21, F31, F41, F51, F61, F71, F81,...
F12, F22, F32, F42, F52, F62, F72, F82,...
myeps <= [t11; t21; t31; t41; t51; t61] <= myinf,...
myeps <= [t12; t22; t32; t42; t52; t62] <= myinf,...
myeps <= [r; rp] <= myinf,...
delta <= deltap <= rp,...
myeps <= ron <= myinf,...
myeps <= roff <= myinf,...
myeps <= deltaz <= rp,...
myeps <= [ron; roff] <= myinf,...
1 <= mu <= myinf,...
myeps <= [kesi1; kesi2] <= myinf,...
myeps <= [ga1; gb1; gp1] <= myinf,...
myeps <= [ga2; gb2; gp2] <= myinf,...
-myinf <= W1(:) <= myinf,...
-myinf <= W2(:) <= myinf,...
t21 >= t11, t22 >= t12,...
t41 >= t31, t42 >= t32,...
t51 >= 1, t52 >=1,...
inv(Qin)*1e-7 <= L <= inv(Qin),...
];
% set the options (commented are the default values)
options=sdpsettings( 'solver','bmibnb',...
'bmibnb.maxtime', 60,...
'bmibnb.maxiter', 200,...
'usex0', 0,...
'penbmi.PBM_MAX_ITER', 50,... %50
'penbmi.UM_MAX_ITER', 1000,... %100
'penbmi.OUTPUT', 1,... %1
'penbmi.DENSE', 0,... %0
'penbmi.LS', 0,... %0
'penbmi.XOUT', 0,... %0
'penbmi.UOUT', 0,... %0
'penbmi.NWT_SYS_MODE', 0,... %0
'penbmi.PREC_TYPE', 0,... %0
'penbmi.DIMACS', 0,... %0
'penbmi.TR_MODE', 0,... %0
'penbmi.U0', 1,... %1
'penbmi.MU', 0.7,... %0.7
'penbmi.MU2', 0.1,... %0.1
'penbmi.P_EPS', 1e-6,... %1e-6
'penbmi.UMIN', 1e-14,... %1e-14
'penbmi.ALPHA', 1e-2,... %1e-2
'penbmi.P0', 1.1,... %1.1
'penbmi.PEN_UP', 0.2,... %0.2
'penbmi.ALPHA_UP', 1.0,... %1.0
'penbmi.PRECISION_2', 1e-7,... %1e-7
'penbmi.CG_TOL_DIR', 5e-2); %5e-2
% 'penbmi.PBM_EPS', 1e-4,... %1e-7
% solve BMI
sol = solvesdp(F,-100*trace(L)+deltap-r,options)