%   Script OPTPEND.M zur Herstellung von Bildern
%
%   Optimalreglerentwurf für das inverse Pendel
%
% verwendet BSPPEND.M, EPSBILD.M
%
% J. Lunze
% 12.12.1996                 Daten wie 15.3.1996
% für 3. Auflage: 19.4.2004
% für 5. Auflage: 13.12.2007
% für 6. Auflage: 5.1.2010
% für 7. Auflage: 31.3.2012
% für 8. Auflage: 3.4.2014
% für 10. Auflage: 11.9.2019
%
%
clear
close all
Bsppend;       % definiert invPendel=ss(A, b, c, d);
%
C=[1 0 0 0          % Wagenposition
   0 1 0 0];        % Winkel
c1T=[1 0 0 0];
c2T=[0 1 0 0];
D = [0; 0];
figure(1);
Q=C'*C;
R=1;


%%
figure(1)
x0=[0; 0.05; 0; 0];    % Anfangsauslenkung: 3 Grad
T=[0:0.02:15];
for q=[0.1, 1, 10]
  K=lqr(A, b, q*Q, R);
  subplot(3,1,1);
  geregeltesPendel=ss(A-b*K, b, C, D);
  Y=initial(geregeltesPendel, x0, T);
  plot(T, Y(:,1),'b');
  hold on;
  subplot(3,1,2);
  plot(T, Y(:,2), 'b');
  hold on;
  subplot(3,1,3);
  Auslenkung3=ss(A-b*K, b, K, 0);
  U=initial(Auslenkung3, x0, T);
  plot(T, U, 'b');
  hold on;
  %   Berechnung der Anteile am G\"utefunktional
  q
  P1=lyap((A-b*K)', c1T'*c1T);
  x0'*P1*x0
  P2=lyap((A-b*K)', c2T'*c2T);
  x0'*P2*x0
end;
subplot(3,1,1);
plot([0 20],[0 0],'k:');
axis([0 15 -0.05 0.3]);     % Maßstab wie BOBPEND.M
latexylabel('$$y_1$$ in m');;
rotateY;
yticks([0 0.3]);
xticks([ 5 10 15 20]);
latextitle('Pendel mit Optimalregler $$(q=0.1, 1, 10)$$');
hold off;
subplot(3,1,2);
plot([0 20],[0 0],'k:');
axis([0 1.5 -0.05 0.05]);
yticks([0 0.05]);
xleer;
latexylabel('$$y_2$$ in rad');;
rotateY;
hold off;
subplot(3,1,3);
plot([0 20],[0 0],'k:');
axis([0 1.5 -10 3]);
xticks([0 0.5 1 1.5]);
yticks([-10 0]);
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in N');
rotateY;
hold off;




%%
%
%   Verbesserte Wichtung der Ausgänge
%
%
figure(2);
Q=C'*diag([1 10000])*C;
for q=[0.1, 1, 10]
  K=lqr(A, b, q*Q, R);
  subplot(3,1,1);
  geregeltesPendel=ss(A-b*K, b, C, D);
  Y=initial(geregeltesPendel, x0, T);
  plot(T, Y(:,1), 'b');
  hold on;
  subplot(3,1,2);
  plot(T, Y(:,2), 'b');
  hold on;
  subplot(3,1,3);
  Auslenkung3=ss(A-b*K, b, K, 0);
  U=initial(Auslenkung3, x0, T);
  plot(T, U, 'b');
  hold on;
    %   Berechnung der Anteile am G\"utefunktional
  q
  P1=lyap((A-b*K)', c1T'*c1T);
  x0'*P1*x0
  P2=lyap((A-b*K)', c2T'*c2T);
  10000*x0'*P2*x0
end;
subplot(3,1,1);
plot([0 20],[0 0],'k:');
axis([0 15 -0.05 0.3]);     % Maßstab wie BOBPEND.M
latexylabel('$$y_1$$ in m');;
rotateY;
yticks([0 0.3]);
xticks([5 10 15 20]);
latextitle('Pendel bei ver\"anderter Wichtungsmatrix');
hold off;
subplot(3,1,2);
plot([0 20],[0 0],'k:');
axis([0 1.5 -0.05 0.05]);
yticks([0 0.05]);
xleer;
latexylabel('$$y_2$$ in rad');;
rotateY;
hold off;
subplot(3,1,3);
plot([0 20],[0 0],'k:');
axis([0 1.5 -10 3]);
xticks([0 0.5 1 1.5]);
yticks([-10 0]);
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in N');
rotateY;
hold off;




%%
%
%   Ortskurve der Determinante der R\"uckf\"uhrdifferenzmatrix
%
%
figure(3);
An=A;
An(3,1)=-0.0001;                 % Verschieben des Pols bei 0
K=lqr(A, b, 10*C'*C, R);
subplot(2,2,1);
Omega=logspace(-7,3,1000);
offeneKette=ss(An, b, K, 0);
[Real, Imag]=nyquist(offeneKette, Omega);
plot(Real(:), Imag(:), 'b', Real(:), -Imag(:),'b--');
hold on
plot([0 0],[-10000 10000],'k:', [-10000 10000],[0 0],'k:');
plot(dcgain(offeneKette), 0, 'bo');
text(-5000, 4000, '\rightarrow');
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-10000 1000 -5500 5000]);
xticks([-10000  0]);
yticks([-5000 0 5000]);
axis('square');
latextitle('Ortskurve der offenen Kette');
%
subplot(2,2,2);
plot(Real(:), Imag(:), 'b', Real(:), -Imag(:),'b--', [0 0],[-10 10],'k:', [-10 10],[0 0],'k:');
hold on
plot([-1 -1],[0.1 -0.1], 'k');
plot(1, 0, 'b*');
text(-1, -1.1, '\rightarrow');
axis([-2.5 0.5 -1.5 1.5])
xticks([-2 -1 0]);
yticks([-1 0 1]);
axis('square');
latexxlabel('Re');
latexylabel('Im');
rotateY;
latextitle('(Ausschnitt)');


figure(4);
subplot(3,3,[1, 2, 4, 5]);
offeneKette=ss(A, b, K, 0);
[P, Z]=pzmap(offeneKette);
for i=1:length(P)
    plot(real(P(:)), imag(P(:)), 'bx');
    hold on
end
for i=1:length(Z)
    plot(real(Z(:)), imag(Z(:)), 'bo');
end
latextitle('PN-Bild der offenen Kette (inverses Pendel und Optimalregler)');
latexylabel('Im');
rotateY;
latexxlabel('Re');
plot([-10 10],[0 0],'k:', [0 0],[-10 10],'k:');
axis([-5 5 -5 5]);
axis('square');
xticks([-5 0 5]);
yticks([-5 0 5]);



%%
%
%    Wurzelortskurve bei verschwindender Wichtung R
%
%
figure(5);
subplot(3,3,[1, 2, 4, 5]);
Pendel=ss(A, b, C, D);
P=pzmap(Pendel);
for k=1:length(P)
    plot(real(P(k)), imag(P(k)), 'bx');
    hold on
end
Q=C'*C;
P=[];
k=1;
Rlog=logspace(-5, 2, 300);
for R=Rlog
    [K, S, E]=lqr(A, b, Q, R);
    P(k,:)=sort(E).';
    k=k+1;
end;
plot(P, 'b');
plot(P(1,1),'bo');
plot(P(1,2),'bo');
plot([-10 10],[0 0],'k:');
plot([0 0],[-10 10],'k:');
text(-7.4, 8, '\leftarrow', 'Rotation', -45);
latextext(-6, 8, '$$\rho \rightarrow 0$$');
text(-7.4, -8, '\leftarrow', 'Rotation', 45);
text(-1.4, 2, '\leftarrow', 'Rotation', -45);
text(-1.6, -2, '\leftarrow', 'Rotation', 45);
axis([-10 10 -10 10]);
xticks([-10 0 10]);
yticks([-10 0 10]);
axis('square');
latextitle('Wurzelortskurve bei ver\"anderter Wichtung R');
latexxlabel('Re');
latexylabel('Im');
rotateY;
hold off;



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