%      NetwPendObserv.m
%
%  Distributed observers for the inverted pendulum
%
%  J. Lunze
%  11.3.2021
%  Version of 3.6.2021
%
echo off
close all
clear


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%
%%  Inverted pendulum:  RT2, S. 111
% State variables: angle, velocity, angular velocity, sensor state, position
%
Tsens=0.6;        % time constant of angle sensor
A = [   0        0         1      0       0
       -0.88    -1.9     0.0056   0       0
       21.5     3.9      -0.14    0       0
      1/Tsens    0         0    -1/Tsens  0
        0        1         0      0       0];
B = [0
     0.3
     -0.62
     0
     0];
C1=[0 0 0 1 0];    % angle (sensor)
C2=[0 0 0 0 1];    % position
D=[0; 0];
n=5;
Pendulum=ss(A, B, [C1; C2], D);

  
%%  Observer design
% 
figure(1)
% for O1: y1=angle, not observable eigenvalue: 0
AB1=A(1:4,1:4);
CB1=C1(1,1:4);
Lambda1=[-4 -5 -6 -10];
L1=(place(AB1', CB1', Lambda1))';
L1=[L1; 0];
% for O2: y2=position, not observable eigenvalue: -1/Tsens
AB2=[A(1:3,1:3) A(1:3,5);
     A(5,1:3) A(5,5)];
CB2=[C2(1,1:3) C2(1,5)];
Lambda2=[-4 -5 -6 -10];  % retain the eigenvalue -1/Tsens
L2=(place(AB2', CB2', Lambda2))';
L2=[L2(1:3); 0; L2(4)];
% Couplings of the observers
kgain1=10;
kgain2=5;
Bz1=[0; 0; 0; 0; 1];   % coupling input vector
Bz2=[0; 0; 0; 1; 0];    
Cz1=[0 0 0 1 0];   % output sent by O1 towards O2
Cz2=[0 0 0 0 1];   % output sent by O2 towards O1
%  Error model
AE=[A-L1*C1-kgain1*Bz1*Cz2  kgain1*Bz1*Cz2;
    kgain2*Bz2*Cz1          A-L2*C2-kgain2*Bz2*Cz1];
BE=[L1 zeros(n,1); zeros(n,1) L2];
CE=eye(2*n,2*n);
DE=zeros(2*n,2);
SigmaE=ss(AE, BE, CE, DE);
%
x0=[0.4; 0; 0; 0; 1];
x10=zeros(n,1);
x10(1,1)=x0(1,1);           % use the measured initial value
x20=zeros(n,1);
x20(5,1)=1;
xE0=[x0-x10; x0-x20];
TE=1;
TimeE=0:0.01:TE;
XE=initial(SigmaE, xE0, TimeE);
%
subplot(4,2,[1 3])
plot(TimeE, XE(:,5), 'b', TimeE, XE(:,10), 'r--', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$e_x$$'); %   in m
rotateY;
yticks([-0.5 0 0.5 1 1.5]);
xticksempty([0 0.5 1 2]);
axis([0 TE -0.5 1]);
latextitle('$${\tt NetwPendObserv}$$ Observation errors');
%
subplot(4,2,[2 4]);
plot(TimeE, XE(:,1), 'b', TimeE, XE(:,6), 'r--', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$e_\phi$$');  %  in rad
rotateY;
xticksempty([0 0.5 1 2]);
yticks([-2 0 2 4 6 8]);
axis([0 TE -2 3]);
%
subplot(4,2,[5 7]);
plot(TimeE, XE(:,2), 'b', TimeE, XE(:,7), 'r--', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$e_{\dot{x}}$$');   %  in $$\frac{\rm m}{\rm s}$$
rotateY;
xticks([0 0.5 1 2]);
yticks([-1 0 1]);
axis([0 TE -1 1]);
%
subplot(4,2,[6 8]);
plot(TimeE, XE(:,3), 'b', TimeE, XE(:,8), 'r--', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$e_{\dot{\phi}}$$');  %   in $$\frac{\rm rad}{\rm s}$$
rotateY; 
xticks([0 0.5 1 2]);
yticks([-20  20 40]);
axis([0 TE -20 20]);

%%  Observation result
%
figure(2)
[Y,TimeE,X]=initial(Pendulum,x0,TimeE);
%
% 
subplot(4,2,[1 3]);
plot(TimeE, X(:,5), 'k--',...
     TimeE, X(:,5)-XE(:,5), 'b', TimeE, X(:,5)-XE(:,10), 'r--', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$x$$'); %   in m
rotateY;
yticks([-1 -0.5 0 0.5 1 1.5 2]);
xticksempty([0 0.5 1 2]);
axis([0 TE 0 1.5]);
latextitle('$${\tt NetwPendObserv}$$ Observation result');
%
subplot(4,2,[2 4]);
plot(TimeE, X(:,1), 'k--',...
     TimeE, X(:,1)-XE(:,1), 'b', TimeE, X(:,1)-XE(:,6), 'r--', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$\phi$$');  %  in rad
rotateY;
xticksempty([0 0.5 1 2]);
yticks([-2 0 2 4 6]);
axis([0 TE -2 6]);
%
subplot(4,2,[5 7]);
plot(TimeE, X(:,2), 'k--',...
     TimeE, X(:,2)-XE(:,2), 'b', TimeE, X(:,2)-XE(:,7), 'r--', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{x}$$');   %  in $$\frac{\rm m}{\rm s}$$
rotateY;
xticks([0 0.5 1 2]);
yticks([-2 -1 0 1 2]);
axis([0 TE -2 1]);
%
subplot(4,2,[6 8]);
plot(TimeE, X(:,3), 'k--',...
     TimeE, X(:,3)-XE(:,3), 'b', TimeE, X(:,3)-XE(:,8), 'r--', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{\phi}$$');  %   in $$\frac{\rm rad}{\rm s}$$
rotateY; 
xticks([0 0.5 1 2]);
yticks([-20 0 20 40]);
axis([0 TE -20 40]);

%%  Application of a stabilising state-feedback controller
%   cf. Example 9.1;   EventtriggeredPendulum.M
%
figure(3)
%  LQ design
C=[C1; C2];       % outputs: angle (sensor), position
R=1;
Q=C'*diag([1 10000])*C;
q=1;
K=lqr(Pendulum,q*Q, R, 0);
%K=lqr(A, B, q*Q, R);   %  Optimalregler aus Optpend.m (RT2, Aufl. 8)
%   closed-loop system
APend=[A-B*K zeros(n,n) B*K;
       zeros(2*n,n)   AE];
BPend=zeros(3*n,1);
CPend=[eye(n,n) zeros(n,2*n)];
DPend=zeros(n,1);
ContrPendulum=ss(APend, BPend, CPend, DPend);
%
xPend0=[x0; xE0];
TP=3;
TimeP=0:0.01:TP;
XPend=initial(ContrPendulum, xPend0, TimeP);
%
subplot(4,2,[1 3])
plot(TimeP, XPend(:,5), 'b', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$x$$'); %   in m
rotateY;
yticks([ 0 1 2]);
xticksempty([0 1 2 3]);
axis([0 TP -0.7 2]);
latextitle('$${\tt NetwPendObserv}$$ Stabilised pendulum');
%
subplot(4,2,[2 4]);
plot(TimeP, XPend(:,1), 'b', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$\phi$$');  %  in rad
rotateY;
xticksempty([0 1 2 3]);
yticks([-2 0 2 4]);
axis([0 TP -2 4]);
%
subplot(4,2,[5 7]);
plot(TimeP, XPend(:,2), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{x}$$');   %  in $$\frac{\rm m}{\rm s}$$
rotateY;
xticks([0 1 2 3]);
yticks([-4 -2 0 2]);
axis([0 TP -4.5 2]);
%
subplot(4,2,[6 8]);
plot(TimeP, XPend(:,3), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{\phi}$$');  %   in $$\frac{\rm rad}{\rm s}$$
rotateY; 
xticks([0 1 2 3]);
yticks([-4 -2 0 2 4]);
axis([0 TP -4 4]);




%%  Communication of the complete observation result
%  different gains for the observable and the unobservable parts
%
figure(5)
% Couplings of the observers
kgain=50;            % gain for the unobservable part of the state vector
ksmall=0.02;           % gain for the observable part 
Bz1=eye(n,n);       %  to use the same formulas as before
Bz2=eye(n,n);
Kgain1=diag([ksmall ksmall ksmall ksmall kgain]);
Kgain2=3*diag([kgain kgain kgain kgain ksmall]);
Cz1=eye(n,n);   % output sent by O1 towards O2
Cz2=eye(n,n);   % output sent by O2 towards O1
%  Error model
AE=[A-L1*C1-Bz1*Kgain1*Cz2  Bz1*Kgain1*Cz2;
    Bz2*Kgain2*Cz1          A-L2*C2-Bz2*Kgain2*Cz1];
BE=[L1 zeros(n,1); zeros(n,1) L2];
CE=eye(2*n,2*n);
DE=zeros(2*n,2);
SigmaE=ss(AE, BE, CE, DE);
XE=initial(SigmaE, xE0, TimeE);
%
subplot(4,2,[1 3])
plot(TimeE, XE(:,5), 'b', TimeE, XE(:,10), 'r--', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$e_x$$'); %   in m
rotateY;
yticks([-0.5 0 0.5]);
xticksempty([0 0.5 1 2]);
axis([0 TE -0.5 0.5]);
latextitle('$${\tt NetwPendObserv}$$ Observation errors');
%
subplot(4,2,[2 4]);
plot(TimeE, XE(:,1), 'b', TimeE, XE(:,6), 'r--', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$e_\phi$$');  %  in rad
rotateY;
xticksempty([0 0.5 1 2]);
yticks([-4 0 4 8]);
axis([0 TE -4 8]);
%
subplot(4,2,[5 7]);
plot(TimeE, XE(:,2), 'b', TimeE, XE(:,7), 'r--', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$e_{\dot{x}}$$');   %  in $$\frac{\rm m}{\rm s}$$
rotateY;
xticks([0 0.5 1 2]);
yticks([-1 -0.5 0]);
axis([0 TE -1 .3]);
%
subplot(4,2,[6 8]);
plot(TimeE, XE(:,3), 'b', TimeE, XE(:,8), 'r--', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$e_{\dot{\phi}}$$');  %   in $$\frac{\rm rad}{\rm s}$$
rotateY; 
xticks([0 0.5 1 2]);
yticks([-20 0 20 40 60]);
axis([0 TE -10 60]);


%%  Observation result
%
figure(6)
subplot(4,2,[1 3]);
plot(TimeE, X(:,5), 'k--',...
     TimeE, X(:,5)-XE(:,5), 'b', TimeE, X(:,5)-XE(:,10), 'r--', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$x$$'); %   in m
rotateY;
yticks([-0.5 0 0.5 1 1.5]);
xticksempty([0 0.5 1 2]);
axis([0 TE 0 1.2]);
latextitle('$${\tt NetwPendObserv}$$ Observation result');
%
subplot(4,2,[2 4]);
plot(TimeE, X(:,1), 'k--',...
     TimeE, X(:,1)-XE(:,1), 'b', TimeE, X(:,1)-XE(:,6), 'r--', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$\phi$$');  %  in rad
rotateY;
xticksempty([0 0.5 1 2]);
yticks([-8 -4 0 4]);
axis([0 TE -8 4]);
%
subplot(4,2,[5 7]);
plot(TimeE, X(:,2), 'k--',...
     TimeE, X(:,2)-XE(:,2), 'b', TimeE, X(:,2)-XE(:,7), 'r--', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{x}$$');   %  in $$\frac{\rm m}{\rm s}$$
rotateY;
xticks([0 0.5 1 2]);
yticks([-1 -0.5 0 0.5 1]);
axis([0 TE -1 1]);
%
subplot(4,2,[6 8]);
plot(TimeE, X(:,3), 'k--',...
     TimeE, X(:,3)-XE(:,3), 'b', TimeE, X(:,3)-XE(:,8), 'r--', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{\phi}$$');  %   in $$\frac{\rm rad}{\rm s}$$
rotateY; 
xticks([0 0.5 1 2]);
yticks([-40 -20 0 20]);
axis([0 TE -40 20]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Comparison to the complete observer
%  Agent 2 sends its measurement y_2 towards Agent 1, which uses the
%  whole output vector
%
figure(7)
% 
CG=[C1; C2];
LambdaG=[-4 -4.5 -5 -6 -10];       
LG=(place(A', CG', LambdaG))';
%
AE=[A-LG*CG];
BE=LG;
CE=eye(n,n);
DE=zeros(n,2);
SigmaE=ss(AE, BE, CE, DE);
xG0=x0-x10-x20;
xG0(5,1)=x0(5,1);                     %  set observer initial position value to zero
xG0(1,1)=x0(1,1);
XE=initial(SigmaE, xG0, TimeE);
%
subplot(4,2,[1 3])
plot(TimeE, XE(:,5), 'b', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$e_x$$'); %   in m
rotateY;
yticks([-0.5 0 0.5 1]);
xticksempty([-0.5 0 0.5 1]);
axis([0 TE -0.5 1]);
latextitle('$${\tt NetwPendObserv}$$ Error of complete observer');
%
subplot(4,2,[2 4]);
plot(TimeE, XE(:,1), 'b', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$e_\phi$$');  %  in rad
rotateY;
xticksempty([0 0.5 1 2]);
yticks([-2 0 2 4]);
axis([0 TE -2 3]);
%
subplot(4,2,[5 7]);
plot(TimeE, XE(:,2), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$e_{\dot{x}}$$');   %  in $$\frac{\rm m}{\rm s}$$
rotateY;
xticks([0 0.5 1 2]);
yticks([-1 0 1]);
axis([0 TE -1 1]);
%
subplot(4,2,[6 8]);
plot(TimeE, XE(:,3), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$e_{\dot{\phi}}$$');  %   in $$\frac{\rm rad}{\rm s}$$
rotateY; 
xticks([0 0.5 1 2]);
yticks([-20 0 20 40 60]);
axis([0 TE -20 20]);

%%  Observation result
%
figure(8)
[Y,TimeE,X]=initial(Pendulum,x0,TimeE);
%
subplot(4,2,[1 3]);
plot(TimeE, X(:,5), 'k--',...
     TimeE, X(:,5)-XE(:,5), 'b',[0 3], [0 0], 'k:');
xleer;
latexylabel('$$x$$'); %   in m
rotateY;
yticks([-1 0 1 2]);
xticksempty([0 0.5 1 1.5]);
axis([0 TE 0 1.5]);
latextitle('$${\tt NetwPendObserv}$$ Complete observer');
%
subplot(4,2,[2 4]);
plot(TimeE, X(:,1), 'k--',...
     TimeE, X(:,1)-XE(:,1), 'b', [0 3], [0 0], 'k:');
xleer;
latexylabel('$$\phi$$');  %  in rad
rotateY;
xticksempty([0 0.5 1 2]);
yticks([-2 0 2 4 6]);
axis([0 TE -2 6]);
%
subplot(4,2,[5 7]);
plot(TimeE, X(:,2), 'k--',...
     TimeE, X(:,2)-XE(:,2), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{x}$$');   %  in $$\frac{\rm m}{\rm s}$$
rotateY;
xticks([0 0.5 1 2]);
yticks([-2 -1 0 1 2]);
axis([0 TE -2 1]);
%
subplot(4,2,[6 8]);
plot(TimeE, X(:,3), 'k--',...
     TimeE, X(:,3)-XE(:,3), 'b', [0 3], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\dot{\phi}$$');  %   in $$\frac{\rm rad}{\rm s}$$
rotateY; 
xticks([0 0.5 1 2]);
yticks([-40 -20 0 20]);
axis([0 TE -20 40]);




%%   Figures
%
epsfigc15('NetwPendObserv');

