%% Structured MIMO Hinf controller design for an experimental single-axis dual-stage platform using HIFOO and HINFSTRUCT
% _Martin Rezac and Zdenek Hurak_
% This m-file first creates a linear state-space model of a real experimental
% motion control platform where two electrical motors are used to rotate a payload around
% a single (vertical) axis. This simple device was built to analyze control
% schemes for inertial stabilization platforms. More details can be found
% in the paper
%
% * M. Rezac and Z. Hurak. Structured MIMO H∞ Design for Dual-Stage Inertial
% Stabilization: Case Study for HIFOO and Hinfstruct Solvers. Mechatronics,
% Vol.X, No.Y, 2013. A draft submitted after a minor revision can be
% downloaded <http://aa4cc.dce.fel.cvut.cz/sites/default/files/downloads/publications/structured-dual-mechatronics.pdf here>.
%
% A rigid plate freely rotatable around a vertical axis carries a standard
% DC brush-type rotary motor, which via a belt transmission drives the secondary
% stage carrying a DC voice-coil motor, which pushes the payload.
% The voice-coil motor enjoys a very small friction, which is a very plausible
% property for inertial stabilization, but its angular stroke is limited to
% (+-5 deg). Therefore, the (geared) DC brush-type rotary motor is used to
% keep the keep the angular devitation close to zero.
% The key disturbance is the angular velocity of the freely rotatable plate
% upon which the stator of the DC brushed motor resides. This mimicks the
% unwanted angular motion of a mobile carrier (car, aircraft, ship).
% The <http://youtu.be/F5N3WkDDRZM video> can help understanding the
% principle.
%
% Having two measured outputs and two control inputs, the control design
% can either be accomplished by successively closing SISO loops or
% designing a MIMO controller. The latter generally cannot include some
% resctrictions on the structure and the order of the controller. With two
% solvers HIFOO and Hinstruct, both providing identical functionality and
% comparable performance, the task of structured MIMO control design can be
% by minimizing H-infinity norm of properly defined generalized plant.
%
% You are encouraged to use PUBLISH functionality in Matlab to obtain a
% nicely formatted version of this code.
%
% But first we need to create the mathematical model of the physical plant
%% Physical parameters
% The subscript 1 refers to the voice-coil motor (VCM), the subscript 2
% refers to the rotary brushed motor.
R1 = 3.6; % [Ohm]
L1 = 1.1e-3; % [H]
kt1 = 5.11; % [N/A]
J1 = 10e-3; % [kg*m^2]
b1 = 0.125; % [N*m*s/rad]
r = 0.065; % [m]
n1 = 74; % [-]
n2 = 4; % [-]
R2 = 3.43; % [Ohm]
L2 = 0.65e-3; % [H]
kt2 = 56.6e-3; % [Nm/A]
Jmp = (67.8e-7+15e-7)*n1^2; % [kg*m^2]
bp = 0.48; % [N*m/(rad/s)]
k = 250; % [N*m/(rad)]
b = 0.9; % [N*m/(rad/s)]
J2 = 25e-3; % [kg*m^2]
voltageVCM_max = 7; % [V]
voltageM_max = 32; % [V]
%% Linear state-space model
% i1, w1, i2, wp, w2, phip, phi2, phi1,
% ---------------------------------------------------------------------------------------------------------------------------------------
A = [ -R1/L1, -r*kt1/L1, 0, 0, r*kt1/L1, 0, 0, 0;
r*kt1/J1, -b1/J1, 0, 0, b1/J1, 0, 0, 0;
0, 0, -R2/L2, -n1*kt2/L2, 0, 0, 0, 0;
0, 0, kt2*n1/Jmp, (-b-bp)/Jmp, b*n2/Jmp, -k/Jmp, k*n2/Jmp, 0;
-kt1*r/J2, 0, 0, b/J2, (-b*n2)/J2, k/J2, -k*n2/J2, 0;
0, 0, 0, 1, 0, 0, 0, 0;
0, 0, 0, 0, 1, 0, 0, 0;
0, 1, 0, 0, 0, 0, 0, 0; ];
B = [voltageVCM_max/L1 0 0;
0 0 0;0, voltageM_max/L2, n2*n1*kt2/L2;
0 0 bp*n2/(n1^2*Jmp);
0 0 0;
0 0 0;
0 0 0;
0 0 0];
C2 = [0, 1, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, -1, 1;
0, 0, 0, 0, 1, 0, 0, 0];
C = [0, 1, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, -1, 1];
D = zeros(2,3);
D2 = zeros(3,3);
Gds = ss(A,B,C,D);
set(Gds,'inputname',{'u_1','u_2','\omega_c'},'outputname',{'\omega_1','e_{phi}'});
Gds = minreal(Gds);
Gds_ext = ss(A,B,C2,D2);
set(Gds_ext,'inputname',{'u_1','u_2','\omega_c'},'outputname',{'\omega_1','e_{phi}','\omega_2'});
Gds_ext = minreal(Gds_ext);
%% Design of a structured MIMO controller for a dual-stage model using HIFOO and Hinfstruct
% Some preliminary stuff
st = tf('s');
fmin=1e-4;
fmax=1e4;
%% Creating the weighting filters for H-infinity synthesis
MS=5;AS=0.001;f=50;
W1=(st/MS+f)/(st+AS*f);
MS=10;AS=0.025;f=1;
W2=(st/MS+f)/(st+AS*f)*100;
AS=1.2;MS=0.6;f=3;
Wc=(st/MS+f)/(st+AS*f)*AS;
Wu1 = 1;
Wu2 = 1;
%% Creating the generalized plant P
w1ref = icsignal(1);wc = icsignal(1);u1 = icsignal(1);u2 = icsignal(1);
zew1 = icsignal(1);zew2 = icsignal(1); zu1 = icsignal(1);zu2 = icsignal(1);
ew1 = icsignal(1); ephi = icsignal(1);
w1 = icsignal(1);
MP = iconnect;
MP.Equation{1} = equate([w1;ephi], Gds*[u1;u2;Wc*wc]);
MP.Equation{2} = equate(ew1,w1ref - w1);
MP.Equation{3} = equate(zew1,W1*ew1);
MP.Equation{4} = equate(zew2,W1*ephi);
MP.Equation{5} = equate(zu1,Wu1*u1);
MP.Equation{6} = equate(zu2,Wu2*u2);
MP.Input = [w1ref; wc; u1; u2];
MP.Output = [zew1; zew2; zu1; zu2; w1ref; ew1; ephi];
P = MP.system;
% group inputs and outputs
NU = 2; NY = 3;
%P = ss(minreal(P));
[r,c]=size(P);
P.InputGroup = struct('U1',1:c-NU,'U2',c-NU+1:c);
P.OutputGroup = struct('Y1',1:r-NY,'Y2',r-NY+1:r);
%% Create the initial controller (for hifoo and hinfstruct)
% K2 controller in state space (init for HIFOO and hinfstruct)
INIT.a = zeros(5);
INIT.d = [ 0 1 0; 0 0 10;];
INIT.b = [ 0 0 0; 0 8 0; 0 0 0; 0 0 2; 0 0 0;];
INIT.c = [ 0 12.5 0 0 0; 0 0 0 2.5 0];
K2 = ss(INIT.a,INIT.b,INIT.c,INIT.d);
%K2=[0 , 1+100/tf('s'), 0; 0 , 0 , (10+5/tf('s'))];
%% HIFOO design
ordR2 = 2;
ordK1 = 2;
ordF = 1;
OPTIONS.struct.a = blkdiag(ones(ordF),ones(ordK1),ones(ordR2));
OPTIONS.struct.b = blkdiag(ones(ordF,1),ones(ordK1,1),ones(ordR2,1));
OPTIONS.struct.c = [zeros(1,ordF), ones(1,ordK1), zeros(1,ordR2); ones(1,ordF), zeros(1,ordK1), ones(1,ordR2)];
OPTIONS.struct.d = [ 0 1 0; 1 0 1];
OPTIONS.nrand = 3;
tic
[K1_hifo, F, VIOL, LOC] = hifoo(P,ordR2+ordK1+ordF,OPTIONS,INIT);
toc
%% Hinfstruct design
ordR2 = 2;
ordK1 = 2;
ordF = 1;
blk = ltiblock.ss('ssblock',5,2,3);
blk.a.Value = INIT.a;
blk.d.Value = INIT.d;
blk.b.Value = INIT.b;
blk.c.Value = INIT.c;
blk.a.Free = OPTIONS.struct.a;
blk.b.Free = OPTIONS.struct.b;
blk.c.Free = OPTIONS.struct.c;
blk.d.Free = OPTIONS.struct.d;
options = hinfstructOptions
%options = hinfstructOptions('Display','iter' )
options = hinfstructOptions('RandomStart',3)
tic
[K4,gamma,info] = hinfstruct(P,blk,options);
toc
K4 = ss(K4);
%% Standard (unstructured, unconstrained) H-infinity design
[K3,CL,GAM] = hinfsyn(P,3,2,'METHOD','lmi');
K3 = minreal(K3);
%% Building closed-loop transfer functions
K1 = K1_hifo;
stability = {'NO'; 'YES'};
% create open and closed loop tf
MG = iconnect;
MG.Equation{1} = equate([w1;ephi], Gds*[u1;u2;wc]);
MG.Equation{2} = equate(ew1,w1ref - w1);
MG.Input = [w1ref; wc; u1; u2];
MG.Output = [w1ref; ew1; ephi];
G = MG.system;
MG.Equation{3} = equate([u1;u2], K2*[w1ref; ew1; ephi]);
MG.Input = [w1ref;wc];
MG.Output = [ew1; ephi; u1; u2];
CL2= minreal(MG.system);
disp(strcat('isstable(CL2): ',stability(isstable(CL2)+1)))
disp(strcat('isstable(K2): ',stability(isstable(K2)+1)))
MG.Equation{3} = equate([u1;u2], K1*[w1ref; ew1; ephi]);
CL= minreal(MG.system);
disp(strcat('isstable(CL) hiffo: ',stability(isstable(CL)+1)))
disp(strcat('isstable(K1) hiffo: ',stability(isstable(K1)+1)))
MG.Equation{3} = equate([u1;u2], K3*[w1ref; ew1; ephi]);
CL3= (MG.system);
disp(strcat('isstable(CL3): ',stability(isstable(CL3)+1)))
disp(strcat('isstable(K3): ',stability(isstable(K3)+1)))
MG.Equation{3} = equate([u1;u2], K4*[w1ref; ew1; ephi]);
CL4= (MG.system);
disp(strcat('isstable(CL4) hinfstruct: ',stability(isstable(CL4)+1)))
disp(strcat('isstable(K4): hinfstruct',stability(isstable(K4)+1)))
%% Printing the values of the resulting Hinf norms
disp(['hifoo = ',num2str(norm(lft(P,K1),inf))])
disp(['PI = ', num2str(norm(lft(P,K2),inf))])
disp(['hinf = ',num2str(norm(lft(P,K3),inf))])
disp(['hinfstruct = ',num2str(norm(lft(P,K4),inf))])
%% Ploting resulting responses
set(CL,'inputname',{'\omega_1^{ref}','\omega_c'},'outputname',{'e_{\omega}','e_{phi}','u_1','u_2'});
set(CL2,'inputname',{'\omega_1^{ref}','\omega_c'},'outputname',{'e_{\omega}','e_{phi}','u_1','u_2'});
set(CL3,'inputname',{'\omega_1^{ref}','\omega_c'},'outputname',{'e_{\omega}','e_{phi}','u_1','u_2'});
set(CL4,'inputname',{'\omega_1^{ref}','\omega_c'},'outputname',{'e_{\omega}','e_{phi}','u_1','u_2'});
set(Gds_ext,'inputname',{'u_1','u_2','\omega_c'},'outputname',{'\omega_1','e_{phi}','\omega_2'});
set(K1,'inputname',{'\omega_1^{ref}','e_{\omega}','e_{phi}'},'outputname',{'u_1','u_2'});
set(K2,'inputname',{'\omega_1^{ref}','e_{\omega}','e_{phi}'},'outputname',{'u_1','u_2'});
set(K3,'inputname',{'\omega_1^{ref}','e_{\omega}','e_{phi}'},'outputname',{'u_1','u_2'});
set(K4,'inputname',{'\omega_1^{ref}','e_{\omega}','e_{phi}'},'outputname',{'u_1','u_2'});
%%
% CL ref -> e
figure(51)
subplot(2,2,1);
step(CL2(1,1),'g',0:0.0001:0.3); title(''); hold on
step(CL3(1,1),'c',0:0.0001:0.3); title(''); hold on
step(CL4(1,1),'m-.',0:0.0001:0.3); title(''); hold on
step(CL(1,1),'k--',0:0.0001:0.3); title(''); hold off
title('');
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
ylabel('angle rate [rad/s]'); grid on
xlabel('time'); %legend('HIFOO','PI','H_{\infty}','Location','Best' )
subplot(2,2,2);
step(-CL2(1,2),'g',0:0.0001:0.3); title(''); hold on
step(CL3(1,2),'c',0:0.0001:0.3); title(''); hold on
step(-CL4(1,2),'m-.',0:0.0001:0.3); title(''); hold on
step(-CL(1,2),'k--',0:0.0001:0.3); title(''); hold off
axis([0,0.3,-0.4,1]);
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
ylabel('angle rate [rad/s]'); grid on
xlabel('time'); %legend('HIFOO','PI','H_{\infty}','Location','Best')
subplot(2,2,3);
step(CL2(2,1),'g',0:0.0001:5.1); title(''); hold on
step(CL3(2,1),'c',0:0.0001:5); title(''); hold on
step(CL4(2,1),'m-.',0:0.0001:5); title(''); hold on
step(CL(2,1),'k--',0:0.0001:5.1); title(''); hold off
axis([0, 5, -0.02, 0.06])
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
ylabel('angle [rad]'); grid on
xlabel('time'); %legend('HIFOO','PI','H_{\infty}','Location','Best')
subplot(2,2,4);
step(CL2(2,2),'g',0:0.0001:5.1); title(''); hold on
step(CL3(2,2),'c',0:0.0001:5); title(''); hold on
step(CL4(2,2),'m-.',0:0.0001:5); title(''); hold on
step(CL(2,2),'k--',0:0.0001:5.1); title(''); hold off
axis([0, 5, -0.02, 0.06])
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
ylabel('-angle [rad]'); grid on
xlabel('time');
h = findall(gcf,'type','text'); set(h,'Fontsize',14,'color','k');
legend('PI','H_{\infty}','Hinfstruct','HIFOO','Location','Best')
h = findall(gcf,'type','axes'); set(h,'ycolor','k','xcolor','k');
h = findall(gcf,'type','line'); set(h,'linewidth',2.5);
%%
figure(11), bodemag(CL2(1:2,1:2),'g',{fmin,fmax}); grid on; hold on;
axis([1e-4,1e4,-100,20]);
bodemag(CL3(1:2,1:2),'c',{fmin,fmax});
axis([1e-4,1e4,-100,20]);
bodemag(CL4(1:2,1:2),'k--',{fmin,fmax});
axis([1e-4,1e4,-100,20]);
bodemag(CL(1:2,1:2),'m-.',{fmin,fmax});
bodemag([1/W1 1/W1;1/W2 1/W2],'r--',{fmin,fmax}); hold off;
legend('PI','H_{\infty}','HIFOO','Hinfstruct','1/W_x','Location','Best');title('');
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
axis([1e-4,1e4,-100,20]);h = gcr; h.AxesGrid.Xunits = 'Hz';
h = findall(gcf,'type','line'); set(h,'linewidth',2.5);
h = findall(gcf,'type','text'); set(h,'Fontsize',14,'color','k');
h = findall(gcf,'type','axes'); set(h,'ycolor','k','xcolor','k','Fontsize',7);
%%
figure(41),bodemag(CL2(3:4,1:2),'g',{fmin,fmax}); grid on; hold on;
bodemag(CL3(3:4,1:2),'c',{fmin,fmax});
bodemag(CL4(3:4,1:2),'k--',{fmin,fmax});
bodemag(CL(3:4,1:2),'m-.',{fmin,fmax});
bodemag(tf([1/Wu1 1/Wu1;1/Wu2 1/Wu2]),'r--',{fmin,fmax}); hold off;
legend('PI','H_{\infty}','HIFOO','Hinfstruct','1/W_{ux}','Location','Best');title('');
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
axis([1e-4,1e4,-100,20]);h = gcr; h.AxesGrid.Xunits = 'Hz';
h = findall(gcf,'type','line'); set(h,'linewidth',2.5);
h = findall(gcf,'type','text'); set(h,'Fontsize',14,'color','k');
h = findall(gcf,'type','axes'); set(h,'ycolor','k','xcolor','k','Fontsize',7);
%%
figure(31), bodemag(K1,'k--',{0.1*fmin,fmax}); grid on; hold on
bodemag(K2,'g',{0.1*fmin,fmax}); hold on
bodemag(K4,'m-.',{0.1*fmin,fmax}); hold on
bodemag(K3,'c',{0.1*fmin,fmax}); hold off
legend('HIFOO','PI','Hinfstruct','H_{\infty}','Location','Best');title('');
h = findobj(gcf,'type','line'); set(h,'linewidth',2.5);
h = gcr; h.AxesGrid.Xunits = 'Hz';
h = findall(gcf,'type','line'); set(h,'linewidth',2.5);
h = findall(gcf,'type','text'); set(h,'Fontsize',14,'color','k');
h = findall(gcf,'type','axes'); set(h,'ycolor','k','xcolor','k','Fontsize',7);