%   Script FFWFBACK.M
%
%   Steuerung in offener Kette und geschlossenem Kreis
%
% J. Lunze
% 23.10.2001
% für 2. Auflage: 5.2.2007
% für 3. Auflage: 21.12.2011
% für 5. Auflage: 18.11.2019
%
%
clear
close all

%%   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
%  Ausschnitt aus dem Modell, der das Pendel beschreibt
A1 = [     0        1
     (m+mw)/mw*g/l  0];
b1 = [   0
     1/mw/l];         % es wird -b verwendet!
c1=[1 0];             % Ausgang = Winkel
d1=[0];
pendel=ss(A1, b1, c1, d1);
%
phi0=30/180*pi;
phi0dot=0;
x0=[phi0; phi0dot];
%
%  Transformation der Zustandsgleichung
[V, D]=eig(A1);
lambda=D(1,1);
btilde=inv(V)*b1;
b=btilde(1,1);
ctilde=c1*V;
c=ctilde(1,1);
Vinv=inv(V);
phi0tilde=Vinv(1,1)*phi0;    % erstes Element des transf. Anfangszustandes


%%  Zeitoptimale Steuerung
%
figure(1)
% Berechnung der optimalen Steuerung: siehe Text "Anmerkungen zu den
% Beispielen"
% Wahl des Umschaltzeitpunktes der Stellgröße
%
tquer=0.7;
t=exp(lambda*tquer);
%
%  Berechnung der Endzeit Te
%  e aus Lösung einer quadratischen Gleichung e^2 + e(2/t-2t) -1 = 0
%
e12=roots([1, 2/t-2*t, -1]);
e=max(e12);
Te=log(e)/lambda;
%
%  Berechnung von uquer
%
uquer=-phi0tilde*lambda*(e*e+1)/(2*e+e*e*(1-2/t)+1-2*t)/b;
%
%
tquerint=ceil(1000*tquer);
Teint=ceil(1000*Te);
Tsim=ceil(1600*Te);               % Verlängerung der Zeitachse um 60%
%  zeitoptimale Steuerung:
Us=uquer*[ones(1,tquerint+1) -ones(1,Teint-tquerint) zeros(1,Tsim-Teint)];
%
subplot(4,1,[1,2]);
Ts=[0:0.001:Tsim/1000];
plot(Ts, Us, 'b');
hold on;
plot([-10 0 0], [0 0 uquer], 'b', [0 0], [-20 20], 'k:', [-20 20], [0 0], 'k:');
plot(tquer, 0, 'bx');
latextext(tquer+0.02, -4, '$$\bar{t}$$');
latextitle('Gesteuertes Pendel bei $$\phi(0)=30$$ Grad');
xleer;
latexylabel('$$u$$ in N');
rotateY;
axis([-0.1 Tsim/1000 -1.1*abs(uquer) 1.1*abs(uquer)]);
yticks([-20 0 20]);
hold off;
%
subplot(4,1,[3,4]);
%   Berechnung des Pendelverhaltens bei zeitoptimaler Steuerung
Ys=lsim(pendel, Us, Ts, x0);
plot(Ts, Ys, 'b');
hold on
plot([-10 0 0], [0 0 Ys(1,1)], 'b', [0 0], [-20 20], 'k:', [-20 20], [0 0], 'k:');
plot(Te, 0, '+');
latextext(Te, -0.1, '$$t_{\rm e}$$');
latexxlabel('$$t$$ in s');
latexylabel('$$\phi$$ in rad');
rotateY;
axis([-0.1 Tsim/1000 -0.4 0.6]);
yticks([-0.4 0 0.4]);
xticks([0 0.5 1 1.5]);
hold off;


%%   Dieselbe Steuerung für einen anderen Anfangszustand
%
figure(2)
x0neu=x0*0.995;   % 0,5% Fehler
%
subplot(4,1,[1,2]);
plot(Ts, Us, 'b', [-10 0 0], [0 0 uquer], 'b', [0 0], [-20 20], 'k:', [-20 20], [0 0], 'k:');
hold on
%
subplot(4,1,[3,4]);
Y=lsim(pendel, Us, Ts, x0neu);
plot(Ts, Y, 'b', [-10 0 0], [0 0 Y(1,1)], 'b',  [0 0], [-20 20], 'k:', [-20 20], [0 0], 'k:');
hold on
%
x0neu=x0*0.9;   % 10% Fehler
%
subplot(4,1,[1,2]);
latextitle('Gesteuertes Pendel bei $$\phi(0)=29,85$$ Grad');
xleer;
latexylabel('$$u$$ in N');
rotateY;
axis([-0.1 Tsim/1000  -1.1*abs(uquer) 1.1*abs(uquer)]);
yticks([-20 0 20]);
%
subplot(4,1,[3,4]);
Y=lsim(pendel, Us, Ts, x0neu);
plot(Ts, Y, 'b--');
latexxlabel('$$t$$ in s');
latexylabel('$$\phi$$ in rad');
rotateY;
axis([-0.1 Tsim/1000 -0.4 0.6]);
yticks([-0.4 0 0.4]);
xticks([0 0.5 1 1.5]);




%%   Vergleich mit dem geregelten System
%    Regler aus RT1, (3. Aufl.), Bsp. 10.3, S. 409
%    aber mit höherer Reglerverstärkung
%
figure(3)
%
% PD-Regler, der den stabilen Pol vollständig kompensiert
%  d.h. Nullstelle des PD-Reglers bei -4.6324 = 1/(TD + T1)
% andere Reglerparameter so gewählt, dass max(u) \approx uquer
%
T1=0.125;
TD=1/lambda-T1;
% Übergangsverhalten des geregelten Systems
%
kP=35;
Ag=[A1-b1*kP*(TD+T1)/T1*[1 0]   b1*kP*TD/T1;
    1/T1 0  -1/T1 ];
bg=[b1*kP*(TD+T1)/T1; -1/T1];
cgphi=[1 0 0];
cgu=[kP*(TD+T1)/T1*[1 0] kP*TD/T1];
dg=[0];
Kreis=ss(Ag, bg, cgphi, dg);
%
subplot(4,1,[3,4]);
[Y, T]=initial(Kreis, [phi0; 0; 0]);
plot(T, Y, 'b', [-10 0 0], [0 0 Y(1,1)], 'b', [0 0], [-20 20], 'k:', [-20 20], [0 0], 'k:');
hold on;
latexylabel('$$\phi$$ in rad');
rotateY;
latexxlabel('$$t$$ in s');
axis([-0.1 Tsim/1000 -0.4 0.6]);
yticks([-0.4 0 0.4]);
xticks([0 0.5 1 1.5]);
%
subplot(4,1,[1,2]);
StellgrKreis=ss(Ag, bg, cgu, dg);
[Ur, T]=initial(StellgrKreis, [phi0; 0; 0]);
plot(T, Ur, 'b', [-10 0 0], [0 0 Ur(1,1)], 'b', [0 0], [-50 50], 'k:', [-20 20], [0 0], 'k:');
hold on;
latexylabel('$$u$$ in N');
rotateY;
xlabel('');
latextitle('Geregeltes Pendel bei $$\phi(0)=30$$ Grad');
axis([-0.1 Tsim/1000  -50 50]);
yticks([-50 0 50]);
xleer;

%%    Aufrichten des Pendels mit einer Regelung (Zustandsrückführung)
%
%
figure(4)
%
%    Zustandsrückführung  u= -(k1 k2)*x
%      Entwurf unter der Forderung, dass der Maximalwert uquer bei t=0
%      angenommen wird, wenn Anfangszustand durch phi0 beschrieben ist
%      Berücksichtigung der Stabilitätsforderung
%
%
%    Wurzelortskurve bezüglich k1tilde
for k1tilde=abs(uquer)/phi0tilde/2:0.01:14
    k2tilde=k1tilde-abs(uquer)/phi0tilde;
    Ag=A1-b1*[k1tilde, k2tilde]*Vinv;
    Dquer=eig(Ag);
    plot(real(Dquer(1)), imag(Dquer(1)), 'b+', real(Dquer(2)), imag(Dquer(2)), 'b+');
    hold on
end
hold off;
latextitle('Wurzelortskurve bez\"uglich $$k_1$$');
xlabel('Re');
ylabel('Im');
rotateY;
axis([-3 0 -1.5 1.5]);
axis('square');
yticks([-2 -1 0 1 2]);
xticks([-3 -2 -1 0]);

%%
%
figure(5)
k1tilde=11.32;
k2tilde=k1tilde-abs(uquer)/phi0tilde;
Ag=A1-b1*[k1tilde, k2tilde]*Vinv;
%eig(Ag)
%[k1tilde, k2tilde]*Vinv*x0
KreisZRF=ss(Ag, b1, c1, 0);
%
subplot(4,1,[3,4]);
[Yr, Tr, X]=initial(KreisZRF, [phi0; 0]);
%                 Steuerung über den verlängerten Zeithorizont der Regelung
Tsim=1000*ceil(max(Tr));
Us=uquer*[ones(1,tquerint+1) -ones(1,Teint-tquerint) zeros(1,Tsim-Teint)];
Ts=[0:0.001:Tsim/1000];
YsoptSt=lsim(pendel, Us, Ts, x0);
plot([Ts 10], [YsoptSt; 0], 'b--', Tr, Yr, 'b', [-10 0 0], [0 0 Yr(1,1)], 'k:', [0 0], [-20 20], 'k:', [-20 20], [0 0], 'k:');
hold on;
latexylabel('$$\phi \;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([-0.1 4 -0.4 0.6]);
yticks([-0.4 0 0.4]);
xticks([0 1 2 3 4]);
%
subplot(4,1,[1,2]);
StellgrKreisZRF=ss(Ag, b1, -[k1tilde, k2tilde]*Vinv, 0);
Ur=initial(StellgrKreisZRF, [phi0; 0], Tr);
plot([Ts 10], [Us 0], 'b--', Tr, Ur, 'b', [-10 0 0], [0 0 Ur(1,1)], 'k:',  [0 0], [-20 20], 'k:', [-20 20], [0 0], 'k:');
hold on;
latexylabel('$$u \;\;$$');
rotateY;
xlabel('');
latextitle('Geregeltes Pendel bei $$\phi(0)=30$$ Grad');
axis([-0.1 4  -1.1*abs(uquer) 1.1*abs(uquer)]);
yticks([-20 0 20]);
xleer;

%%  Dieselbe Regelung für einen anderen Anfangszustand
%
%
figure(6)
x0neu2=x0*0.9;   % 10% Fehler
Yrneu2=initial(KreisZRF, x0neu2, Tr);
Urneu2=initial(StellgrKreisZRF, x0neu2, Tr);
x0neu3=x0*0.8;   % 20% Fehler
Yrneu3=initial(KreisZRF, x0neu3, Tr);
Urneu3=initial(StellgrKreisZRF, x0neu3, Tr);
x0neu4=x0*0.7;   % 30% Fehler
Yrneu4=initial(KreisZRF, x0neu4, Tr);
Urneu4=initial(StellgrKreisZRF, x0neu4, Tr);
x0neu5=x0*0.6;   % 40% Fehler
Yrneu5=initial(KreisZRF, x0neu5, Tr);
Urneu5=initial(StellgrKreisZRF, x0neu5, Tr);
%
subplot(4,1,[3,4]);
plot(Tr, Yr, 'b--', Tr, Yrneu2, 'b', Tr, Yrneu3, 'b', Tr, Yrneu4, 'b',...
     Tr, Yrneu5, 'b', [-10 0 0], [0 0 Yrneu2(1,1)], 'b',...
     [0 0], [-20 20], 'k:', [-20 20], [0 0], 'k:');
hold on;
latexylabel('$$\phi \;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([-0.1 4 -0.4 0.6]);
yticks([-20 0 20]);
xticks([0 1 2 3 4 5]);
%
subplot(4,1,[1,2]);
plot(Tr, Ur, 'b--', Tr, Urneu2, 'b', Tr, Urneu3, 'b', Tr, Urneu4, 'b',...
     Tr, Urneu5, 'b', [-10 0 0], [0 0 Urneu2(1,1)], 'b',...
     [0 0], [-20 20], 'k:', [-20 20], [0 0], 'k:');
hold on;
latexylabel('$$u \;\;$$');
rotateY;
xlabel('');
latextitle('Geregeltes Pendel bei unterschiedlichen Anfangszust\"anden');
axis([-0.1 4  -1.1*abs(uquer) 1.1*abs(uquer)]);
yticks([-20 0 20]);
xleer;

%  Kombination von Vorsteuerung und Regelung ist nur möglich, wenn
%  Stellgrößenbeschränkung vergrößert wird, da bei Realisierung des
%  PD-Reglers aus RT1 eine kritische Reglerverstärkung von kP=35 notwendig
%  ist, was bei phi0=30 Grad auf eine Stellamplitude u(0)=18 führt, also
%  keinen Spielraum für eine Vorsteuerung lässt.

%%     Bilder in .EPS-Dateien
%
Epsbildc15('ffwfb');
