%   Script BOBPEND.M zur Herstellung von Bildern
%
%   Beobachterentwurf für das inverse Pendel
%
%  verwendet BSPPEND.M, EPSBILD.M
%
% J. Lunze
% 19.8.1996        Daten v. 22.3.1996
% für 2. Auflage: 16.12.2001
% für 3. Auflage: 25.3.2004
% für 4. Auflage: 11.1.2006
% für 5. Auflage: 18.12.2007
% für 7. Auflage: 20.4.2012
% für 8. Auflage: 28.3.2014
% für 10. Auflage: 12.9.2019
%
%
clear
close all
Bsppend;
C=[1 0 0 0         % Wagenposition
   0 1 0 0];        % Winkel
r=2;
D = [0; 0];
figure(1);
Q=C'*C;
R=1;
x0=[0; 0.05; 0; 0];    % Anfangsauslenkung: 3 Grad  bisher x0=[0; 0.05; 0; 0];
x0b=[1; 0.1; 0; 0; 0; 0; 0; 0];

%%   Beobachterentwurf
%
%
figure(1);
sigma=[-2 -3 -4 -5];
Lt= place(A', C', sigma);
L=Lt';
Ab=[A zeros(n,n); L*C A-L*C];
Bb=[b;b];
Cb=eye(2*n, 2*n);
D1=0;
T=[0:0.02:15];
Pendel1=ss(Ab, Bb, Cb(1,:), D1);
[X1, T, X]=initial(Pendel1, x0b, T);
subplot(2,2,1);
plot(T, X(:,1), 'b--', T, X(:,5), 'b', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$x$$ in m');
rotateY;
yticks([0 1.5]);
axis([0 1 -0.2 1.5]);
%xticks([]);
%yticks([]);
latextitle('Beobachter f\"ur invertiertes Pendel');
subplot(2,2,2);
plot(T, X(:,2), 'b--', T, X(:,6), 'b', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$\phi$$ in rad');
rotateY;
yticks([0 1]);
axis([0 1 -0.2 1.5]);
yticks([0 1]);
subplot(2,2,3);
plot(T, X(:,3), 'b--', T, X(:,7), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{x}$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
yticks([ 0 0.1]);
axis([0 1 -0.3 .15]);
xticks([0 0.5 1]);
yticks([-0.3 0]);
subplot(2,2,4);
plot(T, X(:,4), 'b--', T, X(:,8), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{\phi}$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
yticks([0 5]);
axis([0 1 -1 10]);
xticks([0 0.5 1]);
yticks([0 8]);


%%   Optimalregler mit "bester" Wichtung entsprechend OPTPEND.M
%
%
figure(2);
Q=C'*diag([1 10000])*C;
q=1;
K=lqr(A, b, q*Q, R);
%
subplot(3,1,1);
Optimalregler1=ss(A-b*K, b, C, 0);
Y=initial(Optimalregler1, x0, T);
plot(T, Y(:,1), 'b', [0 20], [0 0], 'k:');
subplot(3,1,2);
plot(T, Y(:,2), 'b', [0 2], [0 0], 'k:');
%
subplot(3,1,3);
OptimalreglerU=ss(A-b*K, b, K, 0);
[U, T, X]=initial(OptimalreglerU, x0, T);
plot(T, U, 'b', [0 2], [0 0], 'k:');
%
subplot(3,1,1);
axis([0 15 -0.05 0.3]);
xticks([ 5 10 15]);
yticks([0 0.2]);
latexylabel('$$y_1$$ in m');
rotateY;
latextitle('Eigenbewegungen des geregelten Pendels');
hold off;
%
subplot(3,1,2);
axis([0 1.5 -0.05 0.05]);
xleer;
yticks([-0.04 0 0.04]);
latexylabel('$$y_2$$ in rad');
rotateY;
hold off;
%
subplot(3,1,3);
axis([0 1.5 -10 3]);
xticks([0 0.5 1 1.5]);
yticks([-10 -5 0]);
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in N');
rotateY;
hold off;


%%
%
%    Realisierung der Zustandsr\"uckf\"uhrung mit Beobachter
%
%
figure(3);
v=0;
Ag=[A-b*K b*K; zeros(n,n) A-L*C];
Bg=[b*v;zeros(n,1)];
Cg=[C zeros(2,n)];
Dg=D;
x0g=[0; 0.05; 0; 0; 0; -0.05; 0; 0];
subplot(3,1,1);
Beobachter1=ss(Ag, Bg, Cg, D1);
Yb=initial(Beobachter1, x0g, T);
plot(T, Y(:,1), 'b--', T, Yb(:,1), 'b', [0 20], [0 0], 'k:');
axis([0 15 -0.05 0.3]);
xticks([0 5 10 15]);
yticks([0 0.2]);
latexylabel('$$y_1$$ in m');
rotateY;
yticks([0 0.3]);
latextitle('Eigenbewegungen des geregelten Pendels mit Beobachter');
subplot(3,1,2);
plot(T, Y(:,2), 'b--', T, Yb(:,2), 'b', [0 2], [0 0], 'k:');
axis([0 1.5 -0.05 0.05]);
xleer;
yticks([-0.04 0 0.04]);
latexylabel('$$y_2$$ in rad');
rotateY;
yticks([0 0.04]);
subplot(3,1,3);
Beobachter3=ss(Ag, Bg, [K zeros(1,n)], D1);
[Ub, T, X]=initial(Beobachter3, x0g, T);
plot(T, U, 'b--', T, Ub, 'b', [0 2], [0 0], 'k:');
axis([0 1.5 -10 3]);
xticks([0 0.5 1 1.5]);
yticks([-10 -5 0]);
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in N');
rotateY;


%%
%
%    Eigenwerte des mit Beobachter geregelten Systems
%
%
figure(4)
subplot(3,3,[1, 2, 4, 5]);
Beobachter=ss([A-b*K b*K; zeros(n,n) A-L*C], ones(8,1), eye(8,8), zeros(8,1));
P=pzmap(Beobachter);
P=eig(A-b*K);
for k=1:length(P)
    plot(real(P(:)), imag(P(:)), 'bx');
    hold on
end
P=eig(A-L*C);
for k=1:length(P)
    plot(real(P(:)), imag(P(:)), 'bd');
    hold on
end
plot([-10 10],[0 0],'k:', [0 0],[-10 10],'k:');
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-9 1 -5 5]);
axis('square');
xticks([-8 -4 0]);
yticks([-4 0 4]);
latextitle('Eigenwerte des mit Beobachter geregelten Pendesl');


%%   Beobachter mit kleineren Eigenwerten
%
%
figure(5)
sigma=[-4 -5 -6 -7];
Lt= place(A', C', sigma);
L=Lt';
Ag=[A-b*K b*K; zeros(n,n) A-L*C];
Bg=[b*v;zeros(n,1)];
Cg=[C zeros(2,n)];
Dg=D;
subplot(3,1,1);
Beobachter1=ss(Ag, Bg, Cg, D1);
Yb=initial(Beobachter1, x0g, T);
plot(T, Y(:,1), 'b--', T, Yb(:,1), 'b', [0 20], [0 0], 'k:');
axis([0 15 -0.05 0.3]);
xticks([0 5 10 15]);
yticks([0 0.2]);
latexylabel('$$y_1$$ in m');
rotateY;
yticks([0 0.3]);
latextitle('Geregeltes Pendel bei ver\"andertem Beobachter');
subplot(3,1,2);
plot(T, Y(:,2), 'b--', T, Yb(:,2), 'b', [0 2], [0 0], 'k:');
axis([0 1.5 -0.05 0.05]);
xleer;
yticks([-0.04 0 0.04]);
latexylabel('$$y_2$$ in rad');
rotateY;
yticks([0 0.04]);
subplot(3,1,3);
Beobachter3=ss(Ag, Bg, [K zeros(1,n)], D1);
[Ub, T, X]=initial(Beobachter3, x0g, T);
plot(T, U, 'b--', T, Ub, 'b', [0 2], [0 0], 'k:');
axis([0 1.5 -10 3]);
xticks([0 0.5 1 1.5]);
yticks([-10 -5 0]);
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in N');
rotateY;


%%  Reduzierter Beobachter
%
%
figure(6);
K1 = K(1,1:2);
K2 = K(1,3:4);
A11= A(1:2, 1:2);
A12= A(1:2, 3:4);
A21= A(3:4, 1:2);
A22= A(3:4, 3:4);
B=b;
B1= B(1:2, 1);
B2= B(3:4, 1);
%   Entwurf der R\"uckf\"uhrmatrix
sigma=[-2, -3];
Ltr = place(A22', A12', sigma);
Lr = Ltr';
%   Abk\"urzungen
Btilde=B2-Lr*B1;
E = A21 - Lr*A11 + (A22-Lr*A12)*Lr;
K1tilde = K1+K2*Lr;
%   Regelkreis
Ag=[A11-B1*K1tilde      A12       -B1*K2
    A21-B2*K1tilde      A22       -B2*K2
    E-Btilde*K1tilde  zeros(2,2)  A22-Lr*A12-Btilde*K2];
Bg=zeros(6,1);
Cg=[eye(2,2), zeros(2,4)];
Dg=[0;0];
x0g=[0; 0.05; 0; 0; 0; 0];   % x0g=[0; 0.05; 0; 0; 0; 0];
subplot(3,1,1);
Beobachter1=ss(Ag, Bg, Cg, D1);
Yb=initial(Beobachter1, x0g, T);
plot(T, Y(:,1), 'b--', T, Yb(:,1), 'b', [0 20], [0 0], 'k:');
axis([0 15 -0.05 0.3]);
xticks([0 5 10 15]);
yticks([0 0.2]);
latexylabel('$$y_1=x$$ in m');
rotateY;
yticks([0 0.3]);
latextitle('Eigenbewegungen bei reduziertem Beobachter');
subplot(3,1,2);
plot(T, Y(:,2), 'b--', T, Yb(:,2), 'b', [0 2], [0 0], 'k:');
axis([0 1.5 -0.05 0.05]);
xleer;
yticks([-0.04 0 0.04]);
latexylabel('$$y_2$$ in rad');
rotateY;
yticks([0 0.04]);
subplot(3,1,3);
Beobachter3=ss(Ag, Bg, [K1tilde 0 0 K2], D1);
Ub=initial(Beobachter3, x0g, T);
plot(T, U, 'b--', T, Ub, 'b', [0 2], [0 0], 'k:');
axis([0 1.5 -10 3]);
xticks([0 0.5 1 1.5]);
yticks([-10 -5 0]);
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in N');
rotateY;

%%
%
%   Beobachter bei Messrauschen
%
%
figure(7)
%                   Beobachterentwurf wie vorher
sigma=[-2 -3 -4 -5];
Lt= place(A', C', sigma);
L=Lt';
T=[0:0.008:1];
x0=[1; 0.1; 0; 0];    
Pendel1=ss(A, b, C, zeros(2, 1));     %  Pendel aus bsppend mit zwei Messgrößen
[Y, T, X]=initial(Pendel1, x0, T);
R=randn(size(Y))*0.25;
subplot(2, 2, 1)
plot(T, Y(:, 1), 'b--', T, Y(:,1)+R(:, 1), 'b');
latexylabel('$$y_1$$ in m');
yticks([ 0 1.5]);
rotateY;
latexxlabel('$$t$$ in s');
axis([0 1 -0.2 1.5])
%xticks([]);
%yticks([]);
subplot(2, 2, 2)
plot(T, Y(:, 2), 'b--', T, Y(:,2)+R(:, 2), 'b');
latexylabel('$$y_2$$ in rad');
yticks([ 0 1]);
rotateY;
latexxlabel('$$t$$ in s');
axis([0 1 -0.2 1.5])
%xticks([]);
%yticks([]);

figure(8)
AF=A-L*C;
BF=L;
CF=eye(n, n);
DF=zeros(n, r);
Fehlermodell=ss(AF, BF, CF, DF);
x0b=[0; 0; 0; 0];
e0=x0-x0b;
[EY, T, E]=lsim(Fehlermodell, R, T, e0);
subplot(2,2,1);
plot(T, X(:,3), 'b--', T, X(:,3)-E(:,3), 'b', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$\dot{x}$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
yticks([ 0 0.1]);
axis([0 1 -0.3 .15]);
yticks([-0.3 0]);
latextitle('Beobachter f\"ur invertiertes Pendel');
subplot(2,2,2);
plot(T, X(:,4), 'b--', T, X(:,4)-E(:,4), 'b', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$\dot{\phi}$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
yticks([0 5]);
axis([0 1 -1 10]);
yticks([0 8]);
%                         Wiederholung mit den zweiten Eigenwerten
sigma=[-4 -5 -6 -7];
Lt= place(A', C', sigma);
L=Lt';
AF=A-L*C;
BF=L;
Fehlermodell=ss(AF, BF, CF, DF);
[EY, T, E]=lsim(Fehlermodell, R, T, e0);
subplot(2,2,3);
plot(T, X(:,3), 'b--', T, X(:,3)-E(:,3), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{x}$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
yticks([ 0 0.1]);
axis([0 1 -0.3 .15]);
xticks([0 0.5 1]);
yticks([-0.3 0]);
subplot(2,2,4);
plot(T, X(:,4), 'b--', T, X(:,4)-E(:,4), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{\phi}$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
yticks([0 5]);
axis([0 1 -1 10]);
xticks([0 0.5 1]);
yticks([0 8]);





%%      Erstellung der .EPS-Bilderj

%
Epsbildc15('bobpend');
