%% Control of a high-dimensional model of a CD player
% In this seminar, you will try to design a controller for a high-dimensional model of a CD player. Let's get right into it. Load the system matrices stored in CDplayer.mat and define a state-space model of the CD player.

% Load the 120 state model of a CD player
load('CDplayer.mat');
G = ss(full(A), B, C, []);

% The model has 120 states. As a consequence, controllers designed by conventional Hinf design procedures (hinfsyn(), dksyn(), ...) will be of at least 120th order. Such regulators might be very impractical to implement or, what's worse, these design procedures may fail to give you any controller for even higher order models.
% In this seminar, your goal is to reduce the order of the model by various methods (balancmr(), hankelmr(), modred(), ...) and design a (lower-order) controller for the reduced-order model. You will compare the performance of the lower-order controller with a (full-order) controller designed for the full-order model. In addition, you can also try to reduce the order of the full-order controller and compare its performance with the lower-order controller.
%
%% Design a full-order controller by mixsyn for the full-order model

s = zpk('s');
W1 = makeweight(100, 1000, 0.1);
W2 = 1;

W1 = W1*eye(2);
W2 = W2*eye(2);
% W2 = makeweight(.1, 1000, 10);

% It may take some time to design a controller for such a large model...
[K,CL,GAM] = mixsyn(G, W1, W2, []);

figure(1)
step(feedback(K*G, eye(2)), .01)
legend('Full-order controller', 'location', 'SouthEast')

% Notice that the reposnse from first input to the first output is a bit oscillatory (zoom-in the overshoot part of the response). Can you modify the weighting filters so that the response will not be oscillatory any more?
%
%% Reduce the order of the CD player model

% Plot the Hankel singular values
hsv = hankelsv(G);
figure(2)
bar(hsv)

% Based on the plot, choose a desired order of the resulting reduced-order model and reduce the CD player model to the desired order by a method of your choice.

Gr = balancmr(G, 10); % Truncation of balanced realization - better approximation at higher frequencies
% Gr = modred(balreal(G), 10); % Residualization of balanced realization - better approximation at lower frequencies
% Gr = hankelmr(G, 10); % The best theoretical Hinf norm of the error model (G-Gr)

%
% Now compare the original model G to the reduced model Gr. If the frequency characteristics do not match each other, increase the desired order of the reduced-order model and repeat the reduction step.

figure(3)
bode(G,'b',Gr,'r')
grid on
title('Comparing Original (G) to the Reduced model (Gr\_add)')
legend('G - 120-state original ','Gr - reduced','location','northeast')

%
%% Design a controller by mixsyn for the reduced-order model

[Kr,CL,GAM] = mixsyn(Gr, W1, W2, []);

%
% Compare the step responses of the closed loop with the full-order controller and with the reduced-order controller.

figure(1)
step(feedback(K*G, eye(2)), feedback(Kr*G, eye(2)), 'r--', .01)
legend('Full-order controller', 'Reduced-order controller','location', 'NorthEast')

%
% Let us furthermore check whether our design requirements given by weighting filters W1 and W2 are satisfied even when we use the regulator designed on the reduced-order model; that means, whether the sensitivy function S is bellow 1/W1 and whether transfer function K_rS is below 1/W2, where S is a sensitivity function for the closed loop with the original full-order model and the reduced-order controller.

L = G*Kr;
S = 1/(eye(2) + L);
R = Kr*S;
T = 1-S;

figure(4)
sigma(S,'b',R,'r',T,'g',1/W1(1,1),'b-.',ss(1/W2(1,1)),'r-.',{1e-3,1e6})
legend('S','KrS','T','1/W1','1/W2','Location','SouthWest')
grid

%
%% Try to reduce the order of the controller
% Instead of reducing the order of the model, you can also try to reduce the order of the full-order regulator.

% Plot the Hankel singular values and reduce the order of the controller
hsv = hankelsv(K);
figure(5)
bar(hsv)

Kr2 = balancmr(K, 20);

%
% Compare the step responses of the closed loop with the full-order controller and with both reduced-order controllers.

figure(1)
step(feedback(K*G, eye(2)), feedback(Kr*G, eye(2)), 'r--', feedback(Kr2*G, eye(2)), 'g--', .01)
legend('Full-order controller', 'Reduced-order controller', 'Reduced-order controller 2','location', 'NorthEast')

%
% Once again, let us furthermore check whether our design requirements given by weighting filters W1 and W2 are satisfied even when use the reduced-order regulator.

L = G*Kr2;
S = 1/(eye(2) + L);
R = Kr2*S;
T = 1-S;

figure(7)
sigma(S,'b',R,'r',T,'g',1/W1(1,1),'b-.',ss(1/W2(1,1)),'r-.',{1e-3,1e6})
legend('S','Kr2S','T','1/W1','1/W2','Location','SouthWest')
grid

% This example is taken from http://slicot.org/20-site/126-benchmark-examples-for-model-reduction
%

norm(G-Gr,inf) % actual error