%      NetwPendulum.m
%
%  Inverted pendulum with networked angle sensor
%
%  J. Lunze
%  30.10.2017
%  Version of 15.1.2019
% for 2nd edition: 28.3.2021
%
echo off
close all
clear
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%


%%  Inverted pendulum:  RT2
% State variables: Position, velocity,
%                  angle,  angular velocity
figure(1)
echo off
A = [0  1        0        0
     0  -1.9   -0.88    0.0056
     0  0        0        1
     0  3.9     21.5    -0.14];
B = [0 
     0.3
     0
     -0.62];
C=[1 0 0 0; 
   0 0 1 0];   % outputs: position, angle
Cx=[1 0 0 0; 
   0 1 0 0];   % outputs: position, velocity
Cphi=[0 0 1 0; 
      0 0 0 1];   % outputs: angel, angular velocity
D=[0; 0];
Pendulum=ss(A, B, C, D);
%     Discrete-time model
Ts=0.01;
dPendulum=c2d(Pendulum, Ts, 'zoh');
Ad=dPendulum.A;
Bd=dPendulum.B;
%
%   optimum state feedback
kstar=[-1.0 -14.3 -158.0 -26.4];
%   feedback of position and velocity
kx=[-1.0 -14.3 0  0];
kphi=[-158.0 -26.4];
%
%   Closed-loop systems
%   ... with optimum state feedback
A1bar=Ad-Bd*kstar;   % asymptotically stable
%   ... with position feedback only
A0bar=Ad-Bd*kx;    % unstable
%
x0=[0; 0; 0.05; 0];    % Initial angle: 3 degrees
Tend=6;
Y=dinitial(A1bar,Bd,C,D,x0);
Ne=length(Y);
Time=0:Ts:(Ne-1)*Ts;
subplot(4,1,[1 2]);
plot(Time, Y(:,1),'b--');
hold on
subplot(4,1,[3 4])
plot(Time, Y(:,2),'b--');
hold on
%
figure(4)
subplot(3,1,2)
plot(Time, Y(:,1),'b--');
hold on
subplot(3,1,3)
plot(Time, Y(:,2),'b--');
hold on
%
dContrPend=ss(A1bar, Bd, C, D);
dPosContrPend=ss(A0bar, Bd, C, D);



%%  Stability test
p=0.45;
Abar=p*A1bar+(1-p)*A0bar;
eig(Abar);     % stable
QM=[0.1 0 0 0;
    0 0.1 0 0;
    0 0 100 0;
    0 0  0   5];
% QM=[0.1*eye(2,2)   zeros(2,2);
%     zeros(2,2) 20*eye(2,2)];
PM=dlyap(Abar',QM);
EA = PM-p*A1bar'*PM*A1bar - (1-p)*A0bar'*PM*A0bar;
eig(EA)                        % EA has to be positive definite

%%  Behaviour of the inverted pendulum
%
figure(1)
kend=ceil(Tend/Ts);    % number of time points considered
%RandNetwPendulum=rng;                         % save s to restore the generator setting
%save('RandNetwPendulum', 'RandNetwPendulum');
load('RandNetwPendulum');       % Seed (initialisation) of the number generator
rng(RandNetwPendulum);
P=rand(kend+1,1);      % random sequence in [0, 1]
Time=0:Ts:kend*Ts;
X=x0';
P1=[];
Y=[];
U=[];
for k1=1:kend
    if P(k1) >=p
        Aloop=A0bar;
        kloop=kx;
        P1(k1)=0;
    else
        Aloop=A1bar;
        kloop=kstar;
        P1(k1)=1;
    end
    U(k1,1)=-kloop*X(k1,:)';
    Y(k1,:)=(C*Aloop*X(k1,:)')';
    X(k1+1,:)=Aloop*X(k1,:)';
end
U(k1+1,1)=-kloop*X(k1+1,:)';
Y(k1+1,:)=(C*Aloop*X(k1+1,:)')';
if P(k1+1) >=p
        P1(k1+1)=0;
else
        P1(k1+1)=1;
end
% subplot(3,1,1)
% plot(Time, P1,'o');
% axis([0 Tend -0.1 1.1]);
% latextitle(['$${\tt NetwPendulum:}$$ Transmission prob. $$p= $$' num2str(p)]);
% xleer;
% xticks([0 1 2 3 4 5 6 7 8 9 10]);
% yticks([0 1]);
% latexylabel('$$\delta$$');
% rotateY;
% hold off
subplot(4,1,[1 2])
plot(Time, Y(:,1), 'b');
xleer;
yticks([-0.2 0 0.1 0.2]);
latexylabel('$$x$$ in m');
rotateY;
axis([0 Tend -0.02 0.25]);
latextitle(['$${\tt NetwPendulum:}$$ Transmission prob. $$p= $$' num2str(p)]);
hold off
subplot(4,1,[3 4]);
plot(Time, Y(:,2), 'b');
latexxlabel('$$t$$ in s');
xticks([0 1 2 3 4 5 6 7 8 9 10]);
yticks([-0.05 0 0.05]);
latexylabel('$$\phi$$ in rad');
rotateY;
axis([0 Tend -0.06 0.06]);
hold off
%
%  details
figure(2)
subplot(5,1,1)
plot(Time, P1,'bo');
axis([0 1 -0.1 1.1]);
latextitle(['$${\tt NetwPendulum:}$$ Transmission prob. $$p= $$' num2str(p)]);
xleer;
xticks([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 1]);
latexylabel('$$\omega$$');
rotateY;
subplot(5,1,[2, 3])
stairs(Time, U(:,1),'LineWidth',1, 'Color', 'b');
axis([0 1 -9 9]);
xleer;
yticks([-10 -7 0 7 10]);
latexylabel('$$u$$ in N');
rotateY;
subplot(5,1,[4, 5])
%  plot phi again for the shorter time horizon
% plot(Time, Y(:,2));
% axis([0 1 -0.05 0.06]);
% latexxlabel('$$t$$ in s');
% xticks([0 0.2 0.4 0.6 0.8 1]);
% yticks([-0.05 0 0.05]);
% latexylabel('$$\phi$$ in rad');
% rotateY;
%  plot the Lyapunov function: E(V(k+1) - V(k)) < -x'Qx with Q=EA
DeltaV=[];    % V(k+1)-V(k)
V=[];
EV=[];        % x'Qx
for k1=1:kend+1
   V(k1)=X(k1,:)*PM*X(k1,:)';
   EV(k1)=-X(k1,:)*EA*X(k1,:)';
end
DeltaV(1)=0;
for k1=2:kend+1
    DeltaV(k1)=V(k1)-V(k1-1);
end
plot(Time, DeltaV, 'bo', Time, EV, 'b--');
axis([0 1 -3 3]);
latexxlabel('$$t$$ in s');
xticks([0 0.2 0.4 0.6 0.8 1]);
yticks([-3 0 3]);
latexylabel('$$\Delta V(k)$$');
rotateY;

%%    Plot the Lyapunov function
figure(3)
subplot(4,1,[1 2]);
plot(Time, V, 'b');
axis([0 Tend 0 20]);
latexxlabel('$$t$$ in s');
xticks([0 1 2 3 4 5 6]);
yticks([0 10 20]);
latexylabel('$$V$$');
rotateY;
latextitle(['$${\tt NetwPendulum:}$$ Transmission prob. $$p= $$' num2str(p)]);



%%  Inverted pendulum with controller that uses the old input 
%   if the network fails to transmit the new sensor data
p=0.2;
figure(4)
%
%   extended state space:  x - pendulum state
%                          xtilde - sensor data stored
% 
A0extended=[A0bar      -Bd*kphi;
            zeros(2,4) eye(2,2)];  % unstable
A1extended=[A1bar  zeros(4,2); 
            Cphi   zeros(2,2)];    % stable
Cextended=[C zeros(2,2)];
X=[x0; x0(3:4)]';
P1=[];
Y=[];
U1=[];
for k1=1:kend
    if P(k1) >=p
        Aloop=A0extended;
        P1(k1)=0;
        U1(k1,1)=-kstar*[X(k1,1:2) X(k1,5:6)]';
    else
        Aloop=A1extended;
        P1(k1)=1;
        U1(k1,1)=-kstar*X(k1,1:4)';
    end
    Y(k1,:)=(Cextended*Aloop*X(k1,:)')';
    X(k1+1,:)=Aloop*X(k1,:)';
end
Y(k1+1,:)=(Cextended*Aloop*X(k1+1,:)')';
if P(k1+1) >=p
        P1(k1+1)=0;
        U1(k1+1,1)=-kstar*[X(k1+1,1:2) X(k1+1,5:6)]';
else
        P1(k1+1)=1;
        U1(k1+1,1)=-kstar*X(k1+1,1:4)';
end
subplot(3,1,1)
plot(Time, P1,'bo');
axis([0 Tend -0.1 1.1]);
latextitle(['$${\tt NetwPendulum:}$$ Transmission prob. $$p= $$' num2str(p)]);
xleer;
xticks([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 1]);
latexylabel('$$\delta$$');
rotateY;
subplot(3,1,2)
plot(Time, Y(:,1), 'b');
xleer;
xticks([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 0.2]);
latexylabel('$$x$$ in m');
rotateY;
axis([0 Tend 0 0.2]);
subplot(3,1,3)
plot(Time, Y(:,2), 'b');
latexxlabel('$$t$$ in s');
xticks([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 0.05]);
latexylabel('$$\phi$$ in rad');
rotateY;
axis([0 Tend -0.05 0.05]);
hold off
%
%  details
figure(5)
subplot(3,1,1)
plot(Time, P1,'bo');
axis([0 1 -0.1 1.1]);
latextitle(['$${\tt NetwPendulum:}$$ Transmission prob. $$p= $$' num2str(p)]);
xleer;
xticks([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 1]);
latexylabel('$$\delta$$');
rotateY;
subplot(3,1,2)
stairs(Time, U1(:,1))
% hold on
% stairs(Time, U(:,1), 'k--');   % comparison with first method
                                 % time shift of 1 sampling time
                                 % for the input
xleer;
yticks([-7 0 7]);
latexylabel('$$u$$ in N');
rotateY;
axis([0 1 -2 9]);
subplot(3,1,3)
plot(Time, Y(:,2), 'b');
axis([0 1 -0.05 0.05]);
latexxlabel('$$t$$ in s');
xticks([0 0.2 0.4 0.6 0.8 1]);
yticks([-0.05 0 0.05]);
latexylabel('$$\phi$$ in rad');
rotateY;
%

%%
QMext=[100 0 0 0 0 0;
        0 100 0 0  0 0;
        0 0 10000 0 0 0;
        0 0  0   5000 0 0;
        0 0 0 0  300 0;
        0 0 0 0 0  100];
% QM=[0.1*eye(2,2)   zeros(2,2);
%     zeros(2,2) 20*eye(2,2)];
Aext=p*A1extended+(1-p)*A0extended;
PMext=dlyap(Aext',QMext);
EAext = PMext-p*A1extended'*PMext*A1extended - (1-p)*A0extended'*PMext*A0extended;
eig(EAext)                        % EA has to be positive definite

%%  Investigation of the Lyapunov function
%  V(x) = x'(k) P x(k)
%  with  A' P A - P = -Q
figure(6)
Pmat=dlyap(A1bar', diag([0.1 100 1 1]));   % Pmat - solution of the discrete Lyap eqn.
for k1=1:length(X)
    V(k1)=X(k1,1:4)*Pmat*X(k1,1:4)';
end
subplot(2,1,1)
plot(Time, P1,'bo');
axis([0 1 -0.1 1.1]);
latextitle(['$${\tt NetwPendulum:}$$ Transmission prob. $$p= $$' num2str(p)]);
xleer;
xticks([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 1]);
latexylabel('$$\delta$$');
rotateY;
subplot(2,1,2)
stairs(Time, V);
axis([0 1 0 150]);
hold on
%  for comparison: draw the Lyapunov function for the loop without outages
X1=x0';
V1=[];
Aloop=A1bar;
for k1=1:kend
    V1(k1,1)=X1(k1,:)*Pmat*X1(k1,:)';
    X1(k1+1,:)=Aloop*X1(k1,:)';
end
V1(k1+1,1)=X1(k1+1,:)*Pmat*X1(k1+1,:)';
plot(Time, V1, 'b--');
hold off




%%  Inverted pendulum with controller that uses the old input 
%   if the network fails to transmit the new sensor data
%   now: store the old input value
p=0.9;
figure(7)
%
%   extended state space:  x - pendulum state
%                          utilde - old input value  k_phi*(phi, phidot)
% 
A0bar=Ad+Bd*kx;
A0extended=[A0bar      -Bd;
            zeros(1,4)  1];  % unstable
A1extended=[A1bar  zeros(4,1); 
            kphi*Cphi   0 ];    % stable
Cextended=[C zeros(2,1)];
X=[x0; 0]';
P1=[];
Y=[];
U1=[];
for k1=1:kend
    if P(k1) >=p          % no network connection
        Aloop=A0extended;
        P1(k1)=0;
        U1(k1,1)=-kx*X(k1,1:4)'-X(k1,5);
    else
        Aloop=A1extended;
        P1(k1)=1;
        U1(k1,1)=-kstar*X(k1,1:4)';
    end
    Y(k1,:)=(Cextended*Aloop*X(k1,:)')';
    X(k1+1,:)=Aloop*X(k1,:)';
end
Y(k1+1,:)=(Cextended*Aloop*X(k1+1,:)')';
if P(k1+1) >=p
        P1(k1+1)=0;
        U1(k1+1,1)=-kstar*X(k1+1,1:4)'-X(k1,5);
else
        P1(k1+1)=1;
        U1(k1+1,1)=-kstar*X(k1+1,1:4)';
end
subplot(6,1,[1 2])
plot(Time, P1,'bo');
axis([0 Tend -0.1 1.1]);
latextitle(['$${\tt NetwPendulum:}$$ Transmission prob. $$p= $$' num2str(p)]);
xleer;
xticksempty([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 1]);
latexylabel('$$\delta$$');
rotateY;
%
subplot(6,1,[3 4])
plot(Time, Y(:,1), 'b');
xleer;
xticksempty([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 0.2]);
latexylabel('$$x$$ in m');
rotateY;
axis([0 Tend 0 0.2]);
%
subplot(6,1,[5 6])
plot(Time, Y(:,2), 'b');
latexxlabel('$$t$$ in s');
xticks([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 0.05]);
latexylabel('$$\phi$$ in rad');
rotateY;
axis([0 Tend -0.05 0.05]);
hold off
%
%  details
figure(8)
subplot(3,1,1)
plot(Time, P1,'bo');
axis([0 1 -0.1 1.1]);
latextitle(['$${\tt NetwPendulum:}$$ Transmission prob. $$p= $$' num2str(p)]);
xleer;
xticks([0 1 2 3 4 5 6 7 8 9 10]);
yticks([0 1]);
latexylabel('$$\delta$$');
rotateY;
subplot(3,1,2)
stairs(Time, U1(:,1))
% hold on
% stairs(Time, U(:,1), 'k--');   % comparison with first method
                                 % time shift of 1 sampling time
                                 % for the input
xleer;
yticks([-7 0 7]);
latexylabel('$$u$$ in N');
rotateY;
axis([0 1 -2 9]);
subplot(3,1,3)
plot(Time, Y(:,2), 'b');
axis([0 1 -0.05 0.05]);
latexxlabel('$$t$$ in s');
xticks([0 0.2 0.4 0.6 0.8 1]);
yticks([-0.05 0 0.05]);
latexylabel('$$\phi$$ in rad');
rotateY;
%

%%   Convergence test
QMext=[0.1 0 0 0 0 ;
        0 1000 0 0  0 ;
        0 0 1000 0 0 ;
        0 0  0   500 0 ;
        0 0 0 0  0.003];
Aext=p*A1extended+(1-p)*A0extended;
PMext=dlyap(Aext',QMext);
EAext = PMext-p*A1extended'*PMext*A1extended - (1-p)*A0extended'*PMext*A0extended;
eig(EAext)   


%%   Figures
epsfigc('NetwPendulum');