%   Script EventtriggeredPendulum.M
%   
%   Demonstrates the self-triggered control of an inverted pendulum
%   Remark: this is an event-based implementation, not a self-triggered one
%
% J. Lunze  
% 22.12.2014
% from SelfgtriggeredControl.m, version of 3.3.2017
% version of 21.11.2018
% for 2nd edition: 27.4.2021
% 
echo off
clear
close all

%%  Example aus Bsppend.m (RT2, 8. Auflage)
%   Beispiel: Invertiertes Pendel
%
%        Eingang: Kraft an Wagen (Spannung an E-Motor) in N
%        Ausgang: Winkel                               in rad
%
%   Zustandsraummodell mit A, b, c, d
%   (nicht vollständig beobachtbar!)
%
%g = 9.81;  % m/s^2
%mw = 3.2;  % kg
%m = 0.3;   % kg
%l = 0.5;   % m
            % Zeit in s
% Zustände: Position des Wagens, Winkel
%           Geschwindigkeit, Winkelgeschwindigkeit
echo off
A = [0  0        1        0
     0  0        0        1
     0  -0.88   -1.9    0.0056
     0  21.5     3.9    -0.14];
B = [0
     0 
     0.3
     -0.62];
C=[0 1 0 0];
Dist=[0];
n=4;
m=1;
r=1;
invPendel=ss(A, B, C, Dist);
%  discrete-time pendulum model
Ts=0.0003;    % sampling period
invPendeld=c2d(invPendel, Ts);
Ad=invPendeld.A;    % discrete-time model
Bd=invPendeld.B;
Ed=Bd;
% 
C1=[1 0 0 0         % Wagenposition
   0 1 0 0];        % Winkel
R=1;
Q=C1'*diag([1 10000])*C1;
q=1;
K=lqr(invPendel,q*Q, R, 0);
%K=lqr(A, B, q*Q, R);   %  Optimalregler aus Optpend.m (RT2, Aufl. 8)
Abar=A-B*K;
P=are(A, B*inv(R)*B', q*Q);
QV=-(Abar'*P+P*Abar);
%
ClosedLoop=ss(Abar, B, C1, 0);     %  output = y = [position, angle]
ClosedLoopU=ss(Abar, B, -K, 0);  % output = u
%  discrete-time stabilised pendulum model
ClosedLoopd=c2d(ClosedLoop, Ts);
ClosedLoopUd=c2d(ClosedLoopU,Ts);
Abard=ClosedLoopd.A;
Bbard=ClosedLoopd.B;
Ebard=Bbard;

%
x0=[0; 0.18; 0; 0];    % Initial angle approx 10 degrees
%  Disturbance
Tendd=2;
Timed=0:Ts:Tendd';
kendd=length(Timed);
Dist=[];
T0=0;
T1=0.7;
T2=0.85;
T3=1.4;
T4=1.9;
TendS=Tendd;
k0e=1; 
k1e=ceil((T1-T0)/Ts);
k2e=k1e+ceil((T2-T1)/Ts);
k3e=k2e+ceil((T3-T2)/Ts);
k4e=k3e+ceil((T4-T3)/Ts);
k5e=k4e+ceil((TendS-T4)/Ts);
Dist=[0*ones(k1e-k0e,1); 3*ones(k2e-k1e,1); 0*ones(k3e-k2e,1);...
     -2*ones(k4e-k3e,1); 0*ones(kendd-k4e+1,1)];
%  Behaviour of the disturbed continuous control loop
[Ydist,Timed,Xdist]=lsim(ClosedLoop, Dist, Timed, x0);


%%  Behaviour of the continuous state-feedback system
%
%
Tsim=0.01;
Tend=20;
Time=0:Tsim:Tend;
figure(1)
subplot(3, 1, 1);
Ycont=initial(ClosedLoop, x0, Time);
plot(Time, Ycont(:,1), 'b-', [0 Tend], [0 0], 'k:');
subplot(3, 1, 2);
plot(Time, Ycont(:,2), 'b-', [0 Tend], [0 0], 'k:');
subplot(3, 1, 3);
[Ucont, Time, X]=initial(ClosedLoopU, x0, Time);
plot(Time, Ucont, 'b-', [0 2], [0 0], 'k:');
subplot(3, 1, 1);
axis([0 Tend -0.05 0.55]);
latexylabel('$$x$$ in m');
rotateY;
latextitle('$${\tt EventtriggeredPendulum:}$$ Free motion of the pendulum');
xticks([0 5 10 15 20]);
yticks([0 0.5]);
hold off;
subplot(3, 1, 2);
axis([0 Tend/10 -0.03 0.2]);
latexylabel('$$\phi$$ in rad');
rotateY;
xticks([0 1 2]);
yticks([0 0.1 0.2]);
hold off;
subplot(3, 1, 3);
axis([0 Tend/10 -3 30]);
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in N');
rotateY;
xticks([0 1 2]);
yticks([0 30]);
hold off;

%%   Behaviour of the Lyapunov function
figure(2)
%  Determination of V(x)
[Y, Time, X]=initial(ClosedLoop, x0, Time); % continuous closed-loop system
V=[];
for k1=1:length(Time)
    V(k1)=X(k1,:)*P*X(k1,:)';
end
plot(Time, V, 'b', [0 Tend], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$v\;$$');
rotateY;
latextitle('$${\tt EventtriggeredPendulum:}$$ Lyapunov function $$V(t)$$ for the continuous loop');
axis([0 2 0 80]);
yticks([0 20 40 60 80]);
xticks([0 1 2 3 4 5]);


%%  Behaviour of the event-triggered implementation
figure(3)
lambda=1.4;
ebar=0.4;
xold=x0;
u=-K*x0;
Eventtime=[];   % mark the event times on the time axis
V=[];   % Lyapunov function
S=[];   % comparison function: S(t) = V(t_k) exp(-lambda(t-t_k))
U=[];   % input
Y=[];
X(1,:)=x0';   %  state
Y(1,:)=(C1*xold)';
U(1,1)=-K*x0;
tk=0;   % index of last event time
V(1,1)=x0'*P*x0; 
Vk=x0'*P*x0;   % Lyapunov function at last event time
S(1,1)=Vk;
Eventtime(1,1)=1;
for k1=2:kendd
    xnew=Ad*xold+Bd*u;
    X(k1,:)=xnew';
    Y(k1,:)=(C1*xnew)';
    V(k1,1)=xnew'*P*xnew;
    U(k1,1)=u;
    S(k1,1)=Vk*exp(-lambda*(k1-tk)*Ts);
    xold=xnew;
    if V(k1,1)>=S(k1,1)+ebar       % new event occurs
       tk=k1;
       Vk=V(k1,1); 
       S(k1,1)=Vk;
       Eventtime(k1,1)=1;          % mark the event
       u=-K*xold;
    else
       Eventtime(k1,1)=0;
    end
end
subplot(5,1,[1,2])
plot(Timed, Y(:,2), 'b-', [0 Tendd], [0 0], 'k:');
hold on
axis([0 Tendd -0.1 0.2]);
xleer;
latexylabel('$$\phi$$ in rad');
rotateY;
latextitle('$${\tt EventtriggeredPendulum:}$$ Event-triggered control');
latextext(1.5, 0.145, ['$$\lambda=\;$$' num2str(lambda)]);
hold off;
yticks([0 0.1 0.2]);
xticksempty([0 1 2]);
%
subplot(5, 1, [3,4]);
plot(Timed, U, 'b-', [0 2], [0 0], 'k:');
axis([0 Tendd -30 30]);
xleer;
latexylabel('$$u$$ in N');
rotateY;
yticks([-30 0 30]);
xticksempty([0 1 2]);
hold off;
subplot(5, 1, 5);
for k1=1:kendd
    if Eventtime(k1)==1
        plot(k1*Ts, 1, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','w');
        hold on
        plot([k1*Ts k1*Ts],[1 0],'b');
    end
end
yleer;
yticks([]);
latexylabel('events$$\;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tendd 0 1.5]);
xticks([0 1 2]);
hold off;

%
figure(4)
plot(Timed, V, 'b', Timed, S,'b--');
axis([0 Tendd 0 80]);
latexxlabel('$$t$$ in s');
latexylabel('$$v, s\;$$');
rotateY;
yticks([0 20 40 60 80]);
xticks([0 1 2]);
latextext(1.5, 67, ['$$\bar{e}=\;$$' num2str(ebar)]);
hold off;



%%  Behaviour for other parameters
figure(5)
lambda=5.0;
%  Pendulum model
xold=x0;
u=-K*x0;
Eventtime=[];   % mark the event times on the time axis
V=[];   % Lyapunov function
S=[];   % comparison function: S(t) = V(t_k) exp(-lambda(t-t_k))
U=[];   % input
Y=[];
X(1,:)=x0';   %  state
Y(1,:)=(C1*xold)';
U(1,1)=-K*x0;
tk=0;   % index of last event time
V(1,1)=x0'*P*x0; 
Vk=x0'*P*x0;   % Lyapunov function at last event time
S(1,1)=Vk;
Eventtime(1,1)=1;
for k1=2:kendd
    xnew=Ad*xold+Bd*u;
    X(k1,:)=xnew';
    Y(k1,:)=(C1*xnew)';
    V(k1,1)=xnew'*P*xnew;
    U(k1,1)=u;
    S(k1,1)=Vk*exp(-lambda*(k1-tk)*Ts);
    xold=xnew;
    if V(k1,1)>=S(k1,1)+ebar       % new event occurs
       tk=k1;
       Vk=V(k1,1); 
       S(k1,1)=Vk;
       Eventtime(k1,1)=1;          % mark the event
       u=-K*xold;
    else
       Eventtime(k1,1)=0;
    end
end
subplot(5,1,[1,2])
plot(Timed, Y(:,2), 'b-', [0 Tendd], [0 0], 'k:');
axis([0 Tendd -0.1 0.2]);
xleer;
latexylabel('$$\phi$$ in rad');
rotateY;
latextitle('$${\tt EventtriggeredPendulum:}$$ Event-triggered control');
latextext(1.5, 0.145, ['$$\lambda=\;$$' num2str(lambda)]);
hold off;
yticks([-0.1 0 0.1 0.2]);
%xticks([0 1 2]);
subplot(5, 1, [3,4]);
plot(Timed, U, 'b-', [0 2], [0 0], 'k:');
axis([0 Tendd -30 30]);
xleer;
latexylabel('$$u$$ in N');
rotateY;
yticks([-30 0 30]);
%xticks([0 1 2]);
hold off;
subplot(5, 1, 5);
for k1=1:kendd
    if Eventtime(k1)==1
        plot(k1*Ts, 1, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','w');
        hold on
        plot([k1*Ts k1*Ts],[1 0],'b');
    end
end
yleer;
yticks([]);
latexylabel('$$\\$$events$$\;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tendd 0 1.5]);
xticks([0 1 2]);
hold off;


%%  Comparison of continuous and event-based behaviour
figure(5)
subplot(5,1,[1,2])
plot(Time, Ycont(:,2), 'b--', Timed, Y(:,2), 'b-', [0 Tendd], [0 0], 'k:');
axis([0 Tendd -0.1 0.2]);
xleer;
latexylabel('$$\phi$$ in rad');
rotateY;
latextitle('$${\tt EventtriggeredPendulum:}$$ Continuous vs. event-triggered control');
latextext(1.5, 0.145, ['$$\lambda=\;$$' num2str(lambda)]);
hold off;
yticks([0 0.1 0.2]);
%xticks([0 1 2]);
subplot(5, 1, [3,4]);
plot(Time, Ucont, 'b--', Timed, U, 'b-', [0 2], [0 0], 'k:');
axis([0 Tendd -30 30]);
xleer;
latexylabel('$$u$$ in N');
rotateY;
yticks([-30 0 30]);
%xticks([0 1 2]);
hold off;
subplot(5, 1, 5);
for k1=1:kendd
    if Eventtime(k1)==1
        plot(k1*Ts, 1, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','w');
        hold on
        plot([k1*Ts k1*Ts],[1 0],'b');
    end
end
yleer;
yticks([]);
latexylabel('$$\\$$events$$\;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tendd 0 1.5]);
xticks([0 1 2]);
hold off;



%%  Extension for the disturbed system
%  Disturbance = additional force
figure(7)
lambda=3.5;
ebar=0.4;
%
xold=x0;
u=-K*x0;
Eventtime=[];   % mark the event times on the time axis
V=[];   % Lyapunov function
S=[];   % comparison function: S(t) = V(t_k) exp(-lambda(t-t_k))
U=[];   % input
Y=[];
X=[];
X(1,:)=x0';   %  state
Y(1,:)=(C1*xold)';
U(1,1)=-K*x0;
tk=0;   % index of last event time
V(1,1)=x0'*P*x0; 
Vk=x0'*P*x0;   % Lyapunov function at last event time
S(1,1)=Vk;
Eventtime(1,1)=1;
for k1=2:kendd
    xnew=Ad*xold+Bd*u+Bd*Dist(k1);
    X(k1,:)=xnew';
    Y(k1,:)=(C1*xnew)';
    V(k1,1)=xnew'*P*xnew;
    U(k1,1)=u;
    S(k1,1)=Vk*exp(-lambda*(k1-tk)*Ts);
    xold=xnew;
    if V(k1,1)>=S(k1,1)+ebar       % new event occurs
       tk=k1;
       Vk=xold'*P*xold;
       S(k1,1)=Vk;
       Eventtime(k1,1)=1;          % mark the event
       u=-K*xold;
    else
       Eventtime(k1,1)=0;
    end
end
subplot(9,1,[1,2]);
dmin=1.2*min(Dist);
dmax=1.2*max(Dist);
fill([k1e*Ts k2e*Ts k2e*Ts k1e*Ts k1e*Ts],[dmin dmin dmax dmax dmin],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
hold on
fill([k3e*Ts k4e*Ts k4e*Ts k3e*Ts k3e*Ts],[dmin dmin dmax dmax dmin],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
plot(Timed, Dist, 'b');
latextitle('$${\tt EventtriggeredPendulum:}$$ Event-triggered with disturbance');
axis([0 Tendd dmin dmax]);
xleer;
latexylabel('$$d$$');
rotateY;
hold off;
yticks([-2 0 2]);
%
subplot(9,1,[3,4,5])
plot(Timed, X(:,2), 'b-', [0 Tendd], [0 0], 'k:');
hold on
plot(Timed, Ydist(:,2), 'b--');
axis([0 Tendd -0.1 0.2]);
xleer;
latexylabel('$$\phi$$ in rad');
rotateY;
latextext(1.5, 0.145, ['$$\lambda=\;$$' num2str(lambda)]);
hold off;
yticks([ 0 0.1]);
%xticks([0 1 2]);
hold off;
%
subplot(9,1,[6,7,8]);
plot(Timed, U, 'b-', [0 Tend], [0 0], 'k:');
axis([0 Tendd -30 30]);
xleer;
latexylabel('$$u$$ in N');
rotateY;
yticks([0 30]);
%xticks([0 1 2]);
hold off;
%
subplot(9,1,9);
for k1=1:kendd
    if Eventtime(k1)==1
        plot(k1*Ts, 1, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','w');
        hold on
        plot([k1*Ts k1*Ts],[1 0],'b');
    end
end
yleer;
yticks([]);
latexylabel('$$\\$$events$$\;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tendd 0 1.5]);
xticks([0 1 2]);
hold off;
%
figure(8)
fill([k1e*Ts k2e*Ts k2e*Ts k1e*Ts k1e*Ts],[-0.2 -0.2 80 80 -0.2],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
hold on
fill([k3e*Ts k4e*Ts k4e*Ts k3e*Ts k3e*Ts],[-0.2 -0.2 80 80 -0.2],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
plot(Timed, V, 'b', Timed, S,'b--');
axis([0 Tendd 0 80]);
latexxlabel('$$t$$ in s');
latexylabel('$$v, s\;$$');
rotateY;
yticks([0 20 40 60 80]);
xticks([0 1 2]);
latextext(1.5, 67, ['$$\bar{e}=\;$$' num2str(ebar)]);
hold off;



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Event-triggered state feedback for the inverted pendulum
%    without disturbance estimation
%
figure(9)
ebar=0.005;
Wx=diag([1 1 0.03 0.05]);       %diag([25 70 5 1]);
%     vector norm = max |(Wx x)_i|
Tsevent=Ts;    % sampling time for the event-triggered control loop
Timeevent=0:Tsevent:Tendd;
kendevent=length(Timeevent);
%
subplot(9,1,[1,2]);
plot(Timed, Dist, 'b');
hold on
%
X=[];
Xs=[];
Dapprox=[];
Test=[];
Eventtime=[];
U=[];
X(1,:)=x0';
Xs(1,:)=x0';
Dapprox=zeros(kendevent,1);
Eventtime=zeros(kendevent,1);
Eventtime(1,1)=1;
for k1=2:kendevent
    Test(k1-1)=max(Wx*[abs(X(k1-1,1)-Xs(k1-1,1)); abs(X(k1-1,2)-Xs(k1-1,2));...
                  abs(X(k1-1,3)-Xs(k1-1,3)); abs(X(k1-1,4)-Xs(k1-1,4))]);
   if Test(k1-1)>=ebar
          % reset the state of the input generator
       Xs(k1,:)=Abard*X(k1-1,:)'+Ebard*Dapprox(k1-1,1);
       U(k1-1,:)=-K*X(k1-1,:)';
       if Wx(1,1)*abs(X(k1-1,1)-Xs(k1-1,1))>=ebar
          Eventtime(k1-1,1)=1;   % event generated by x_1
       end
       if Wx(2,2)*abs(X(k1-1,2)-Xs(k1-1,2))>=ebar
          Eventtime(k1-1,1)=2;   % event generated by x_2
       end
       if Wx(3,3)*abs(X(k1-1,3)-Xs(k1-1,3))>=ebar
          Eventtime(k1-1,1)=3;   % event generated by x_3
       end
       if Wx(4,4)*abs(X(k1-1,4)-Xs(k1-1,4))>=ebar
          Eventtime(k1-1,1)=4;   % event generated by x_4
       end
   else
       Xs(k1,:)=Abard*Xs(k1-1,:)'+Ebard*Dapprox(k1-1,1);
       U(k1-1,:)=-K*Xs(k1-1,:)';
   end
   X(k1,:)=Ad*X(k1-1,:)' +Bd*U(k1-1,:)' +Ed*Dist(k1-1,1);
end
U(kendevent,:)=-K*Xs(kendevent,:)';
subplot(9,1,[1,2]);
plot(Timeevent, Dapprox, 'b--');
latexylabel('$$d\;$$');
rotateY;
xleer;
yticks([-2 2]);
axis([0 Tendd 1.1*min(Dist) 1.1*max(Dist)]);
latextitle('$${\tt EventtriggeredPendulum:}$$ Event-triggered control');
hold off
subplot(9,1,[3,4,5])
plot(Timeevent, X(:,2),'b', Timeevent, Xs(:,2), 'b--', [0 Tendd],[0 0],'k:');
hold on
plot([0 Tendd],[0 0],'k:');
latexylabel('$$\phi$$ in rad');
rotateY;
hold off;
yticks([0 0.1]);
xleer;
axis([0 Tendd -0.05 0.2]);
hold off
subplot(9,1,[6,7,8])
plot(Timeevent, U, 'b', [0 Tendd],[0 0],'k:');
latexylabel('$$u$$ in N');
rotateY;
yticks([ 0 30]);
xleer;
axis([0 Tendd -6 30]);
hold off
subplot(9,1,9)
for k1=1:kendevent
    if Eventtime(k1)~=0
        plot(k1*Tsevent, 1, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','w');
        hold on
        plot([k1*Tsevent k1*Tsevent],[1 0],'b');
    end
end
axis([0 Tendd 0 1.5]);
yleer;
yticks([]);
latexylabel('events$$\;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 0.7 1 2]);
hold off



%%   Event-triggered state feedback for the inverted pendulum
%    with disturbance estimation
%
figure(10)
DistPendulum=ss(A, B, eye(4,4), 0);  %  model for the determination of H
%
subplot(9,1,[1,2]);
plot(Timed, Dist, 'b');
hold on
%
X=[];
Xs=[];
Dapprox=[];
Test=[];
Eventtime=[];
H=[];
X(1,:)=x0';
Xs(1,:)=x0';
Dapprox(1,1)=0;
told=0;
Eventtime=zeros(kendevent,1);
Eventtime(1,1)=1;
for k1=2:kendevent
    Test(k1-1)=max(Wx*[abs(X(k1-1,1)-Xs(k1-1,1)); abs(X(k1-1,2)-Xs(k1-1,2));...
                  abs(X(k1-1,3)-Xs(k1-1,3)); abs(X(k1-1,4)-Xs(k1-1,4))]);
   if Test(k1-1)>=ebar
       tnew=(k1-1)*Ts;   % new event time
       %  determination of H for singular A
       [YD,TimeD]=step(DistPendulum, tnew-told);
       H=YD(end,:)';
       Hinv=inv(H'*H)*H';
       Dapprox(k1,1)=Dapprox(k1-1,1)+Hinv*(X(k1-1,:)-Xs(k1-1,:))';
       told=tnew;
         % reset the state of the input generator
       Xs(k1,:)=Abard*X(k1-1,:)'+Ebard*Dapprox(k1-1,1);
       U(k1-1,:)=-K*X(k1-1,:)';
       if Wx(1,1)*abs(X(k1-1,1)-Xs(k1-1,1))>=ebar
          Eventtime(k1-1,1)=1;   % event generated by x_1
       end
       if Wx(2,2)*abs(X(k1-1,2)-Xs(k1-1,2))>=ebar
          Eventtime(k1-1,1)=2;   % event generated by x_2
       end
       if Wx(3,3)*abs(X(k1-1,3)-Xs(k1-1,3))>=ebar
          Eventtime(k1-1,1)=3;   % event generated by x_3
       end
       if Wx(4,4)*abs(X(k1-1,4)-Xs(k1-1,4))>=ebar
          Eventtime(k1-1,1)=4;   % event generated by x_4
       end
   else
       Xs(k1,:)=Abard*Xs(k1-1,:)'+Ebard*Dapprox(k1-1,1);
       U(k1-1,:)=-K*Xs(k1-1,:)';
       Dapprox(k1,1)=Dapprox(k1-1,1);
   end
   X(k1,:)=Ad*X(k1-1,:)' +Bd*U(k1-1,:)' +Ed*Dist(k1-1,1);
end
subplot(9,1,[1,2]);
plot(Timeevent, Dapprox, 'b--');
latexylabel('$$d\;$$');
rotateY;
xleer;
yticks([-2 2]);
axis([0 Tendd 1.1*min(Dist) 1.1*max(Dist)]);
latextitle('$${\tt EventtriggeredPendulum:}$$ Event-triggered control');
hold off
subplot(9,1,[3,4,5])
plot(Timeevent, X(:,2),'b', Timeevent, Xs(:,2), 'b--', [0 Tendd],[0 0],'k:');
hold on
plot([0 Tendd],[0 0],'k:');
latexylabel('$$\phi$$ in rad');
rotateY;
hold off;
yticks([0 0.1]);
xleer;
axis([0 Tendd -0.05 0.2]);
hold off
subplot(9,1,[6,7,8])
plot(Timeevent, U, 'b', [0 Tendd],[0 0],'k:');
latexylabel('$$u$$ in N');
rotateY;
yticks([0 30]);
xleer;
axis([0 Tendd -6 30]);
hold off
subplot(9,1,9)
for k1=1:kendevent
    if Eventtime(k1)~=0
        plot(k1*Tsevent, 1, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','w');
        hold on
        plot([k1*Tsevent k1*Tsevent],[1 0],'b');
    end
end
axis([0 Tendd 0 1.5]);
yleer;
yticks([]);
latexylabel('events$$\;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 1 2]);
hold off


%%  Ultimate boundedness of the event-triggered implementation
%   use the same algorithm as in figure(3)
%
figure(11)
lambda=1.4;
ebar=0.4;
xold=x0;
u=-K*x0;
%  New time horizon
Tendlong=40;
Timelong=0:Ts:Tendlong;
kendlong=length(Timelong);
Eventtime=[];   % mark the event times on the time axis
V=[];   % Lyapunov function
S=[];   % comparison function: S(t) = V(t_k) exp(-lambda(t-t_k))
U=[];   % input
Y=[];
X(1,:)=x0';   %  state
Y(1,:)=(C1*xold)';
U(1,1)=-K*x0;
tk=0;   % index of last event time
V(1,1)=x0'*P*x0; 
Vk=x0'*P*x0;   % Lyapunov function at last event time
S(1,1)=Vk;
Eventtime(1,1)=1;
for k1=2:kendlong
    xnew=Ad*xold+Bd*u;
    X(k1,:)=xnew';
    Y(k1,:)=(C1*xnew)';
    V(k1,1)=xnew'*P*xnew;
    U(k1,1)=u;
    S(k1,1)=Vk*exp(-lambda*(k1-tk)*Ts);
    xold=xnew;
    if V(k1,1)>=S(k1,1)+ebar       % new event occurs
       tk=k1;
       Vk=V(k1,1); 
       S(k1,1)=Vk;
       Eventtime(k1,1)=1;          % mark the event
       u=-K*xold;
    else
       Eventtime(k1,1)=0;
    end
end
subplot(9,1,[1,2])
plot(Timelong, V, 'b', Timelong, S,'b--');
axis([0 8 0 10]);
xleer;
latexylabel('$$v, s\;$$');
rotateY;
yticks([0  10]);
latextext(6, 7.5, ['$$\bar{e}=\;$$' num2str(ebar)]);
hold off;
latextitle('$${\tt EventtriggeredPendulum:}$$ Ultimate boundedness');
%
subplot(9,1,[3,4,5])
plot(Timelong, Y(:,2), 'b-', [0 Tendlong], [0 0], 'k:');
hold on
axis([0 8 -0.08 0.08]);
xleer;
latexylabel('$$\phi$$ in rad');
rotateY;
latextext(6, 0.05, ['$$\lambda=\;$$' num2str(lambda)]);
hold off;
yticks([-0.05 0.05]);
%
subplot(9,1,[6,7,8]);
plot(Timelong, U, 'b-', [0 Tendlong], [0 0], 'k:');
axis([0 8 -20 20]);
xleer;
latexylabel('$$u$$ in N');
rotateY;
yticks([-10 0 10]);
hold off;
subplot(9,1,9);
for k1=1:kendlong
    if Eventtime(k1)==1
        plot(k1*Ts, 1, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','w');
        hold on
        plot([k1*Ts k1*Ts],[1 0],'b');
    end
end
yleer;
yticks([]);
latexylabel('events$$\;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 8 0 1.5]);
xticks([0 2 4 6 8 10]);
hold off;

%%  Representation in the state space
%
figure(12)
plot(X(:,1), X(:,2), 'b');
hold on
plot(x0(1,1), x0(2,1),'bo');
plot([-0.03 0.03 0.03 -0.03 -0.03],[-0.03 -0.03 0.03 0.03 -0.03],'k--');
axis([-0.1 0.35 -0.1 0.2]);
latexylabel('$$\phi$$');   % in rad
latexxlabel('$$x$$');  % in m
rotateY;
latextext(0.02, 0.17, '$$\mathbf{x}_0$$');
latextext(0, 0.05, '$${\cal A}$$');
yticks([-0.1 0 0.1 0.2]);
xticks([-0.1 0 0.1 0.2 0.3]);
hold off


%%  Demonstrate the ultimate boundedness of the undisturbed pendulum
%   Algorithm of figure(9)
%   with vanishing disturbance Dist=0
%   longer time horizon:  Tendlong
figure(13)
ebar=0.005;
Wy=diag([1 1 1 1]);
Wx=diag([1 2 0.03 0.05]);       %diag([25 70 5 1]);
%   Determination of emax
%     vector norm = max |(Wx x)_i|
%     induced matrix norm = max_i  sum_j |g_ij|
Errorsys=ss(A-B*K, B*K*inv(Wx), eye(4,4)*Wy, 0);
[G,TimeG]=impulse(Errorsys);
for k1=1:length(TimeG)
    Gnorm(k1,1)=max(sum(abs([G(k1,1,1) G(k1,1,2) G(k1,1,3) G(k1,1,4);
                             G(k1,2,1) G(k1,2,2) G(k1,2,3) G(k1,2,4);
                             G(k1,3,1) G(k1,3,2) G(k1,3,3) G(k1,3,4);
                             G(k1,4,1) G(k1,4,2) G(k1,4,3) G(k1,4,4)])'));
    Gnorm4(k1,:,:)=abs([G(k1,1,1) G(k1,1,2) G(k1,1,3) G(k1,1,4);
                        G(k1,2,1) G(k1,2,2) G(k1,2,3) G(k1,2,4);
                        G(k1,3,1) G(k1,3,2) G(k1,3,3) G(k1,3,4);
                        G(k1,4,1) G(k1,4,2) G(k1,4,3) G(k1,4,4)]);
end
TsG=TimeG(2)-TimeG(1);
emax=ebar*trapz(Gnorm4)*TsG;
%  New time horizon
Tendlong=40;
Timelong=0:Ts:Tendlong;
kendlong=length(Timelong);
%
X=[];
Xs=[];
Test=[];
Eventtime=[];
U=[];
X(1,:)=x0';
Xs(1,:)=x0';
Eventtime=zeros(kendlong,1);
Eventtime(1,1)=1;
for k1=2:kendlong
    Test(k1-1)=max(Wx*[abs(X(k1-1,1)-Xs(k1-1,1)); abs(X(k1-1,2)-Xs(k1-1,2));...
                  abs(X(k1-1,3)-Xs(k1-1,3)); abs(X(k1-1,4)-Xs(k1-1,4))]);
   if Test(k1-1)>=ebar
          % reset the state of the input generator
       Xs(k1,:)=Abard*X(k1-1,:)';
       U(k1-1,:)=-K*X(k1-1,:)';
       if Wx(1,1)*abs(X(k1-1,1)-Xs(k1-1,1))>=ebar
          Eventtime(k1-1,1)=1;   % event generated by x_1
       end
       if Wx(2,2)*abs(X(k1-1,2)-Xs(k1-1,2))>=ebar
          Eventtime(k1-1,1)=2;   % event generated by x_2
       end
       if Wx(3,3)*abs(X(k1-1,3)-Xs(k1-1,3))>=ebar
          Eventtime(k1-1,1)=3;   % event generated by x_3
       end
       if Wx(4,4)*abs(X(k1-1,4)-Xs(k1-1,4))>=ebar
          Eventtime(k1-1,1)=4;   % event generated by x_4
       end
   else
       Xs(k1,:)=Abard*Xs(k1-1,:)';
       U(k1-1,:)=-K*Xs(k1-1,:)';
   end
   X(k1,:)=Ad*X(k1-1,:)' +Bd*U(k1-1,:)';
end
U(kendlong)=-K*Xs(kendlong,:)';
subplot(10,1,[1,2,3])
plot(Timelong, X(:,1),'b', Timelong, Xs(:,1), 'b--', [0 Tendlong],[0 0],'k:');
hold on
plot([0 Tendlong],[0 0],'k:');
latexylabel('$$\phi$$ in rad');
rotateY;
%yticks([0 0.1]);
xleer;
%axis([0 Tendlong -0.05 0.2]);
hold off
latextitle('$${\tt EventtriggeredPendulum:}$$ Ultimate boundedness');
subplot(10,1,[4,5,6])
plot(Timelong, X(:,2),'b', Timelong, Xs(:,2), 'b--', [0 Tendlong],[0 0],'k:');
hold on
plot([0 Tendlong],[0 0],'k:');
latexylabel('$$\phi$$ in rad');
rotateY;
%yticks([0 0.1]);
xleer;
%axis([0 Tendlong -0.05 0.2]);
hold off
subplot(10,1,[7,8,9])
plot(Timelong, U, 'b', [0 Tendlong],[0 0],'k:');
latexylabel('$$u$$ in N');
rotateY;
%yticks([ 0 30]);
xleer;
%axis([0 Tendlong -6 30]);
hold off
subplot(10,1,10)
for k1=1:kendlong
    if Eventtime(k1)~=0
        plot(k1*Ts, 1, 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','w');
        hold on
        plot([k1*Ts k1*Ts],[1 0],'b');
    end
end
%axis([0 Tendlong 0 1.5]);
yleer;
yticks([]);
latexylabel('events$$\;\;$$');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 1 2]);
hold off




%%
epsfigc15('EventtriggeredPendulum');