%   Script LJAPST.M
%
%   Stabilitätsanalyse mit Ljapunowfunktion
%   (Illustration der Bewegungsgleichung)
%
% J. Lunze
% 27.4.2003
% für 2. Auflage: 27.12.2006
% für 3. Auflage: 3.12.2011
% für 4. Auflage: 5.1.2016
% für 5. Auflage: 30.12.2019
%
%
clear
close all
%               PARAMETER
m=3;            % kg  Masse
l=1;            % m   Pendellänge
kw=1.5;           % Ns/rad  Widerstandsbeiwert für Luft- und Reibungswiderstand
g=9.81;         % kgm/s/s  Erdbeschleunigung
                % Kräfte in N
                % Zeit in Sekunden
                % Weg in m
                % Winkelgeschwindigkeit in rad/s,
                %         also Ausgangsgröße y=Winkelgeschwindigkeit in rad/s
                %
                % LINEARISIERTES MODELL
A=[0 1; -g/l -kw/m/l];
B=[0; 0];
C=[0 1];       % Ausgang = Winkelgeschwindigkeit
D=0;
Pendel=ss(A, B, C, D);
%
Tend=9;
dt=0.01;
T=[0:dt:Tend];

%%    Eigenbewegung des linearisierten Pendels
%
figure(1)
x0=[30/180*pi; 0];      % 30 Grad Anfangsauslenkung
%
subplot(4,1,[1,2]);
[Y, T, X]=initial(Pendel, x0, T);
plot(T, X(:,1), 'b', [0 15], [0 0],'k:');
latextitle('Eigenbewegung des Pendels');
latexylabel('$$\phi$$ in rad')
rotateY;
%xleer;
xticksempty([0 2 4 6 8 10]);
axis([0 7 -0.5 0.6]);
yticks([-0.4 0 0.4]);
%
subplot(4,1,[3,4]);
plot(T, X(:,2),  'b',[0 15], [0 0],'k:');
latexylabel('$$\frac{\mathrm{d} \phi}{\mathrm{d} t}$$ in $$\frac{\rm rad}{\rm s}$$')
rotateY;
latexxlabel('$$t$$ in s');
%xleer;
axis([0 7 -1.5 1.5]);
yticks([-1 0 1]);
xticks([0 2 4 6 8]);

%%    Eigenbewegung des nichtlinearen Pendels
%
figure(2)
dt=(T(2)-T(1))/20;
Tnlin=[0:dt:7];
kendnlin=length(Tnlin);
Xnlin=x0';
for k=1:kendnlin-1
    Xnlin(k+1,1)=Xnlin(k,1)+Xnlin(k,2)*dt;
    Xnlin(k+1,2)=Xnlin(k,2)-(g/l*sin(Xnlin(k,1))+kw/m/l*Xnlin(k,2))*dt;
end
%
subplot(4,1,[1,2]);
plot(Tnlin, Xnlin(:,1), 'b', T, X(:,1),'b--', [0 15], [0 0],'k:')
latextitle('Eigenbewegung des nichtlinearen Pendels');
latexylabel('$$\phi$$ in rad')
rotateY;
xleer;
xticksempty([0 2 4 6 8 10]);
axis([0 7 -0.5 0.6]);
yticks([-0.4 0 0.4]);
%
subplot(4,1,[3,4]);
plot(Tnlin, Xnlin(:,2), 'b', T, X(:,2),'b--', [0 15], [0 0],'k:')
latexylabel('$$\frac{\mathrm{d} \phi}{\mathrm{d} t}$$ in $$\frac{\rm rad}{\rm s}$$')
rotateY;
latexxlabel('$$t$$ in s');
axis([0 7 -1.5 1.5]);
yticks([-1 0 1]);
xticks([0 2 4 6 8 10])

% Die Abbildung zeigt, dass es für die hier verwendeten Parameter
% keine wesentlichen Abweichungen des linearisierten vom nichtlinearen Modell
% gibt. Deshalb wird mit den Trajektorien des linearen Modells weitergerechnet.



%%   Berechnung der Ableitung der Energiefunktion entlang der Trajektorie
%
figure(3)
%
kend=size(X);
for k=1:kend
    v(k)=m/2*l*l*X(k,2)*X(k,2)+m*g*l*(1-cos(X(k,1)));
    vPunkt(k)=-kw*l*X(k,2)*X(k,2);
end
%
subplot(4,1,[1,2]);
plot(T, v,  'b');
latextitle('Verhalten der Ljapunowfunktion');
latexylabel('$$v$$')
rotateY;
%latexxlabel('$$t$$ in s');
xleer;
xticksempty([0 2 4 6 8 10]);
yticks([0 2 4]);
axis([0 7 0 4.5]);
%yticks([]);
%
subplot(4,1,[3,4]);
plot(T, vPunkt, 'b', [0 15], [0 0],'k:')
latexylabel('$$\frac{\mathrm{d}v}{\mathrm{d}t}$$')
rotateY;
yticks([-4 -2 0]);
latexxlabel('$$t$$ in s');
axis([0 7 -4 0.5])
yticks([-4 -2 0]);
xticks([0 2 4 6 8 10]);

%%  Veranschaulichung der Pendelbewegung im "Energiefeld" der Ljapunowfunktion
%
figure(4)
x1min=-0.8;
x2min=-1.8;
Punktezahl=100;
X1=[];
X2=[];
V=[];
d1=2*abs(x1min)/Punktezahl;
d2=2*abs(x2min)/Punktezahl;
for k1=1:Punktezahl
    for k2=1:Punktezahl
        X1(k1,k2)=x1min+k1*d1;
        X2(k1,k2)=x2min+k2*d2;
        V(k1,k2)=m/2*l*l*X2(k1,k2)*X2(k1,k2)+m*g*l*(1-cos(X1(k1,k2)));
    end
end
mesh(X1, X2, V)
hold on
plot3(X(:,1), X(:,2), v(:), 'k');
%plot3(X(:,1), X(:,2), v, '*');
latexxlabel('$$\phi$$ in rad');
%yticks([-1 0 1]),
latexylabel('$$\dot{\phi}$$ in $${\rm rad/s}$$')
rotateY;
latexzlabel('$$v$$');
handleOnZLabel=get(gca, 'Zlabel');                    % Handle auf Beschriftung der z-Achse setzen
set(handleOnZLabel,'rotation',0);                     % Beschriftung waagerecht ausrichten
set(handleOnZLabel,'HorizontalAlignment','right');    % rechtsbündig positionieren
view(-60, 70)
axis([-0.4 0.4 -1.3 1.3 0 11]);
yticks([-1 0 1]);
xticks([-0.4 0 0.4]);
zticks([0 10]);
hold off

%%  Pendelbewegung im Zustandsraum mit den "Höhenlinien" der Ljapunowfunktion
%
figure(5)
contour(X1, X2, V, 6, 'b')
hold on
plot(X(1:75,1), X(1:75,2), 'r');
latextitle('Pendelbewegung im Zustandsraum');
latexxlabel('$$\phi$$ in rad')
latexylabel('$$\frac{\mathrm{d} \phi}{\mathrm{d} t}$$ in $$\frac{\rm rad}{\rm s}$$')
rotateY;
yticks([-1 0 1]);
xticks([-0.5 0 0.5]);


%%    Verwendung einer neuen Ljapunowfunktion für das lineare Modell
%
figure(6)
x0=[10/180*pi; 0];      % 10 Grad Anfangsauslenkung, für die das lineare Modell gilt
[Y, T, X]=initial(Pendel, x0, T);
Q=0.3*[10 3; 3 10];
P=lyap(A', Q);
v=[];
vPunkt=[];
%
for k=1:length(X)
    v(k)=X(k,:)*P*X(k,:)';
    vPunkt(k)=-X(k,:)*Q*X(k,:)';
end
%
subplot(4,1,[1,2]);
plot(T, v, 'b');
latextitle('Verhalten der neuen Ljapunowfunktion');
latexylabel('$$v$$')
rotateY;
xleer;
xticksempty([0 2 4 6 8 10]);
axis([0 7 0 1]);
yticks([0 0.5 1]);
%
subplot(4,1,[3,4]);
plot(T, vPunkt, 'b', [0 15], [0 0],'k:')
latexylabel('$$\frac{\mathrm{d}v}{\mathrm{d}t}$$')
rotateY;
latexxlabel('$$t$$ in s');
yticks([-0.5 0]);
axis([0 7 -0.8 0.15]);
yticks([-0.5 0]);
xticks([0 2 4 6 8 10]);

%%  Veranschaulichung der Pendelbewegung im "Potentialfeld" der Ljapunowfunktion
%
figure(7)
x1min=-0.5;
x2min=-1.5;
Punktezahl=100;
d1=2*abs(x1min)/Punktezahl;
d2=2*abs(x2min)/Punktezahl;
for k1=1:Punktezahl
    for k2=1:Punktezahl
        X1(k1,k2)=x1min+k1*d1;
        X2(k1,k2)=x2min+k2*d2;
        x=[X1(k1,k2); X2(k1,k2)];
        V(k1,k2)=x'*P*x;
    end
end
mesh(X1, X2, V)
latexxlabel('$$\phi$$ in rad')
latexylabel('$$\frac{\mathrm{d} \phi}{\mathrm{d} t}$$ in $$\frac{\rm rad}{\rm s}$$')
rotateY;
latexzlabel('$$v$$');
handleOnZLabel=get(gca, 'Zlabel');                    % Handle auf Beschriftung der z-Achse setzen
set(handleOnZLabel,'rotation',0);                     % Beschriftung waagerecht ausrichten
set(handleOnZLabel,'HorizontalAlignment','right');    % rechtsbündig positionieren
yticks([-1 0 1]);
xticks([-0.5 0 0.5]);
zticks([0 10]);


%%   EPS-Bilder
%
Epsbildc15('ljapst');
