%   Script UnobservPend.M
%
%   State observation with model uncertainties
%   Demonstrate that parameter changes make the observation error model
%   completely observable
%
% J. Lunze
% 25.10.2021
% homepage version of 27.2.2023
% 
%
clear
close all



%%   Inverted pendulum
%    RT1, Beispiel 10.5
%
figure(1);
g = 9.81;   % m/s^2
m = 0.3;      % kg
l = 0.5;    % m
lhat=0.7;
            % Time in s
Tend=0.7;
%
A = [     0        1
     3*g/2/l  0];
B = [   0
     3/m/l^2];       
C = [1 0];             % Output = angle
D = 0;
n=2;
Pendulum=ss(A, B, C, D);
%
phi0=10/180*pi;       %  10 degree initial angle
phidot0=0;
x0=[phi0; phidot0];
Time=0:0.01:Tend;
%
[Y,Time,X]=initial(Pendulum,x0,Time);
subplot(4,1,[1,2]);
plot(Time, X(:,1), 'b');
xlabel('');
ylabel('\phi in rad');
axis([0 Tend 0 4]);
title('Behaviour of the pendulum');
%
subplot(4,1,[3,4]);
plot(Time, X(:,2), 'b');
xlabel('t  in s');
ylabel('phidot');
axis([0 Tend 0 20]);

%%  State observer with the precise model
%
figure(2)
%  eigenvalues of the pendulum: \pm 5.425
LambdaB=[-8, -10];   % Eigenvalues of the observer
K=place(A', C', LambdaB);
L=K';
AB=A-L*C;
%  Model of the overall system (pendulum + observer)
Abar=[A zeros(n,n); 
      L*C A-L*C];
Bbar=[B; B];
Cbar=[C -C];   % Output = observation error y - yhat
Dbar=0;
PendulumObserv=ss(Abar, Bbar, Cbar, Dbar);
xbar0=[x0; zeros(n,1)];
[Y,Time,X]=initial(PendulumObserv,xbar0,Time);
subplot(6,1,[1,2]);
plot(Time, X(:,1), 'b--', Time, X(:,3), 'b');
xlabel('');
ylabel('\phi in rad');
axis([0 Tend 0 4]);
title('State observation result');
%
subplot(6,1,[3,4]);
plot(Time, X(:,2), 'b--', Time, X(:,4), 'b');
xlabel('');
ylabel('phidot');
axis([0 Tend 0 22]);
%
subplot(6,1,[5, 6]);
plot(Time, Y, 'b', [0 Tend], [0 0], 'k:');
xlabel('t  in s');
ylabel('phi-phihat in rad');
axis([0 Tend -0.05 0.22]);


%%  State observer with changed length of the bar
%
figure(3)
Ahat = [     0        1
     3*g/2/lhat  0];
Bhat = [   0
     3/m/lhat^2];       
K=place(Ahat', C', LambdaB);
L=K';
%  Model of the overall system (pendulum + observer)
Anew=[A zeros(n,n); 
      L*C Ahat-L*C];
Bnew=[Bhat; Bhat];
Cbar=[C -C];   % Output = observation error y - yhat
Dbar=0;
PendulumObservnew=ss(Anew, Bnew, Cbar, Dbar);
[Y,Time,X]=initial(PendulumObservnew,xbar0,Time);
subplot(6,1,[1,2]);
plot(Time, X(:,1), 'b--', Time, X(:,3), 'b');
xlabel('');
ylabel('\phi in rad');
axis([0 Tend 0 4]);
title('Observer with changed parameter');
%
subplot(6,1,[3,4]);
plot(Time, X(:,2), 'b--', Time, X(:,4), 'b');
xlabel('');
ylabel('phidot');
axis([0 Tend 0 22]);
%
subplot(6,1,[5, 6]);
plot(Time, Y, 'b', [0 Tend], [0 0], 'k:');
xlabel('t  in s');
ylabel('phi-phihat in rad');
axis([0 Tend -0.05 0.22]);


