%   Script OPTGUETE.M zur Herstellung von Bildern
%
%   Gütegebirge für den Optimalreglerentwurf für das inverse Pendel
%
% verwendet BSPPEND.M, EPSBILD.M
%
% J. Lunze
% für 4. Auflage: 11.1.2006
% für 5. Auflage: 18.10.2007
% für 7. Auflage 25.5.2012
% für 10. Auflage: 12.9.2019
%
%
clear
close all
%   Daten wie in "Automatisierungstechnik", Beispiel 2.6
%   Beispiel: Invertiertes Pendel (RT1. (3. Aufl.) Beispiel 10.3, S. 409)
%
%
g = 9.81;   % m/s^2
mw = 3.2;   % kg
m = 0.3;    % kg
l = 0.5;    % m
            % Zeit in s
%  Pendel aus RT1
A  = [
           0        1
       21.5  0];
b = [0
     0.62];         
cT=[1 0];             % Winkel phi
d =[0];
Pendel=ss(A, b, cT, d);
%
phi0=30/180*pi;      % Anfangsauslenkung: 30 Grad
phi0dot=0;

%%       Gütegebirge
%
figure(1);
Q=3000*cT'*cT;       %  Faktor 1000, um Anteile von y(t) und u(t) in dieselbe Größenordnung zu bringen
R=1;
x0=[phi0; phi0dot];
K1=[];
K2=[];
I=[];
l1=0;
l2=0;
%                            Optimalregler kstern=[99  17.8]
%                            Gütewert 315
for k1=50:1:150
    l1=l1+1;
    l2=0;
for k2=5:1:45
    l2=l2+1;
    K1(l1, l2)=k1;
    K2(l1, l2)=k2;
    k=[k1; k2];             % Zustandsrückführung
    Aquer=A-b*k';
    Qquer=Q+R*k*k';
    P=lyap(Aquer', Qquer);  % Aquer' statt Aquer siehe Ljapunowgl. in "help lyap"
    I(l1, l2)=x0'*P*x0;
end
end
mesh(K1, K2, I);
view([-65, 11]);
axis([55 100 6 30 200 700]);
hold on
kstern=lqr(Pendel, Q, R);
plot3(kstern(1,1), kstern(1,2), 315, 'b*');
plot3(kstern(1,1), kstern(1,2), 200, 'b*');
plot3([kstern(1,1) kstern(1,1)],[kstern(1,2) kstern(1,2)],[200 315]);
latexxlabel('$$k_1$$');     %  in Nm/rad
latexylabel('$$k_2$$');    %  in Nms/rad
rotateY;
latexzlabel('$$J$$');
rotateZ;
hold off
xticks([60 80 100]);
yticks([0 10 20 30]);
zticks([200 400 600]);


%%
%
%  Simulationsuntersuchungen
%
%
figure(2)
T=[0:0.01:2];
kstern=lqr(Pendel, Q, R);
subplot(6,1,[1,2]);
geregeltesPendel=ss(A-b*kstern, b, cT, d);
Y=initial(geregeltesPendel, x0, T);
plot(T, Y, 'b', [0 2], [0 0],'k:');
axis([0 1.4 -0.2 0.6]);
yticks([0 0.5]);
latextitle('Pendelwinkel');
xleer;
latexylabel('$$\phi$$ in rad');
rotateY;
%  veränderte Regler
subplot(6,1,[3,4]);
k2=[95; 8]
geregeltesPendel=ss(A-b*k2', b, cT, d);
Y=initial(geregeltesPendel, x0, T);
plot(T, Y, 'b', [0 2], [0 0],'k:');
axis([0 1.4 -0.2 0.6]);
xleer;
latexylabel('$$\phi$$ in rad');
rotateY;
yticks([0 0.5]);
subplot(6,1,[5,6]);
k3=[60; 25]
geregeltesPendel=ss(A-b*k3', b, cT, d);
Y=initial(geregeltesPendel, x0, T);
plot(T, Y, 'b', [0 2], [0 0],'k:');
axis([0 1.4 -0.2 0.6]);
latexxlabel('$$t$$ in s');
latexylabel('$$\phi$$ in rad');
rotateY;
yticks([0 0.5]);
xticks([0 0.5 1]);
%
%  Eintragen der neuen Regler in das G\"utegebirge
figure(1)
hold on
%   für k=k2
Aquer=A-b*k2';
Qquer=Q+R*k2*k2';
P=lyap(Aquer', Qquer); 
I2=x0'*P*x0;
plot3(k2(1,1), k2(2,1), I2, 'bd');
plot3(k2(1,1), k2(2,1), 200, 'bd');
plot3([k2(1,1) k2(1,1)],[k2(2,1) k2(2,1)],[200 I2],'b');
%   für k=k3
Aquer=A-b*k3';
Qquer=Q+R*k3*k3';
P=lyap(Aquer', Qquer); 
I3=x0'*P*x0;
plot3(k3(1,1), k3(2,1), I3, 'b+');
plot3(k3(1,1), k3(2,1), 200, 'b+');
plot3([k3(1,1) k3(1,1)],[k3(2,1) k3(2,1)],[200 I3],'b');
hold off

%% Stellgrößen
%
figure(3)
subplot(6,1,[1,2]);
geregeltesPendelu=ss(A-b*kstern, b, -kstern, d);
U=initial(geregeltesPendelu, x0, T);
plot(T, U, 'b', [0 2], [0 0],'k:');
axis([0 1.4 -50 20]);
yticks([-40 0]);
latextitle('Stellgr\"ossen');
xleer;
latexylabel('$$u$$ in Nm');
rotateY;
subplot(6,1,[3,4]);
geregeltesPendelu=ss(A-b*k2', b, -k2', d);
U=initial(geregeltesPendelu, x0, T);
plot(T, U, 'b', [0 2], [0 0],'k:');
axis([0 1.4 -50 20]);
yticks([-40 0]);
xleer;
latexylabel('$$u$$ in Nm');
rotateY;
subplot(6,1,[5,6]);
geregeltesPendelu=ss(A-b*k3', b, -k3', d);
U=initial(geregeltesPendelu, x0, T);
plot(T, U, 'b', [0 2], [0 0],'k:');
axis([0 1.4 -50 20]);
yticks([-40 0]);
xticks([0 0.5 1]);
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in Nm');
rotateY;

%%   Berechnung der G\"utewerte
%   J=Jy+Ju   Zerlegung in die beiden Integrale für y(t) und u(t)
%
%
kstern
Aquer=A-b*kstern;
Qu=R*kstern'*kstern;
Py=lyap(Aquer', Q);
Pu=lyap(Aquer', Qu);
Jy=x0'*Py*x0
Ju=x0'*Pu*x0
Jy+Ju
k2
Aquer=A-b*k2';
Qu=R*k2*k2';
Py=lyap(Aquer', Q);
Pu=lyap(Aquer', Qu);
Jy=x0'*Py*x0
Ju=x0'*Pu*x0
Jy+Ju
k3
Aquer=A-b*k3';
Qu=R*k3*k3';
Py=lyap(Aquer', Q);
Pu=lyap(Aquer', Qu);
Jy=x0'*Py*x0
Ju=x0'*Pu*x0
Jy+Ju

%%   Gütegebirge bei veränderter Anfangsbedingung
%
%
figure(4)
phi0=0;      % Anfangsauslenkung: 0 Grad
phi0dot=3;   % Anfangswinkelgeschwindigkeit: 5 rad/s
x0=[phi0; phi0dot];
K1=[];
K2=[];
I=[];
l1=0;
l2=0;
%                            Optimalregler kstern=[99  17.8]
%                            G\"utewert 256
for k1=50:1:150
    l1=l1+1;
    l2=0;
for k2=5:1:45
    l2=l2+1;
    K1(l1, l2)=k1;
    K2(l1, l2)=k2;
    k=[k1; k2];             % Zustandsr\"uckf\"uhrung
    Aquer=A-b*k';
    Qquer=Q+R*k*k';
    P=lyap(Aquer', Qquer);  % Aquer' statt Aquer siehe Ljapunowgl. in "help lyap"
    I(l1, l2)=x0'*P*x0;
end
end
mesh(K1, K2, I);
view([-65, 11]);
axis([55 100 6 30 200 700]);
hold on
kstern=lqr(Pendel, Q, R);
plot3(kstern(1,1), kstern(1,2), 256, 'b*');
plot3(kstern(1,1), kstern(1,2), 200, 'b*');
plot3([kstern(1,1) kstern(1,1)],[kstern(1,2) kstern(1,2)],[200 256]);
latexxlabel('$$k_1$$');   %  in Nm/rad
latexylabel('$$k_2$$');   %  in Nms/rad
rotateY;
latexzlabel('$$J$$');
rotateZ;
hold off
xticks([60 80 100]);
yticks([0 10 20 30]);
zticks([200 400 600]);



%%     Ausgabe der Bilder als .eps-Dateien
%
Epsbildc15('optguete');
