clear all
close all
clc

M=10;    %mass of the cart          [kg]
m=2;     %mass of the pendulum      [kg]
l=1;     %lenght                    [m]
ra=10;   %armature resistance       [ohm]
rr=.1;   %radius of the wheels      [m]
ke=2;    %constant                  [N*m/A]
kc=2;    %constant                  [V*s/rad]
g=9.81;
T = 0.1; %discretization period     [s]

%% Define the matrices of the discretized system

A = [0  1  0      0;
     0 -4 -1.9620 0;
     0  0  0      1;
     0  4 11.7720 0];

B = [0;
     0.2;
     0;
    -0.2];

[Ad,Bd] = c2d(A,B,T);
 

Cd = [1 0 0 0
     0 0 1 0];
 
Dd = [0;
      0];

%% (a) - FUll-order Luenberger observer (with delay)

% Check that the discretized system is observable
rank(obsv(Ad,Cd))      % = 4

% Design observer
% P = [exp(-99.7*T) exp(-99.8*T) exp(-99.9*T) exp(-100*T)];
P = [exp(-9.7*T) exp(-9.8*T) exp(-9.9*T) exp(-10*T)];
% P = [1e-4 0 0 -1e-6];
L = place(Ad',Cd',P)';    % = [0.9202 -0.0153; 1.0334 -0.2403;
                           ... 0.3371 1.3717; 3.0413 5.7673]
                               
% Check that the eigenvalues are placed correctly
eig(Ad-L*Cd)           % = [0.3791 0.3679 0.3753 0.3716]' 
                        ... = [exp(-9.7*T) exp(-10*T) exp(-9.8*T) exp(-9.9*T)]'
                            
% Simulate with the linearized and discretized system (Ad,Bd,Cd,Dd)
sysd = ss(Ad,Bd,Cd,Dd,T);
obsd = ss(Ad-L*Cd,[Bd L],eye(4),[],T);

time = 0:T:2;
u = zeros(size(time));
x0 = [0 0 1e-4 0]';
xhat0 = [0 0 1e-1 0]';

% With for loop iteration

% x = zeros(4,length(time)+1);
% y = zeros(2,length(time)+1);
% xhat = zeros(4,length(time)+1);
% x(:,1) = x0;
% xhat(:,1) = xhat0;
% y(:,1) = Cd*x0;
% 
% for k = 1:length(time)
%     
%     x(:,k+1) = sysd.A*x(:,k) + sysd.B*u(k);
%     y(:,k+1) = sysd.C*x(:,k+1);
%     
%     xhat(:,k+1) = obsd.A*xhat(:,k) + obsd.B*([u(k); y(:,k)]);
%     
% end
% 
% x = x(:,1:end-1)';
% xhat = xhat(:,1:end-1)';
    
% With lsim

[y,t,x] = lsim(sysd,u,time,x0);

[~,t,xhat] = lsim(obsd,[u; y'],time,xhat0);

% Draw Plots
ylabelz = {'Linear Position','Linear Velocity','Angular Position','Angular Velocity'};
figure(1);
figure(2);
for i = 1:4
   
    figure(1);
    subplot(2,2,i)
    grid on
    hold on
    plot(time,x(:,i),'LineWidth',2,'Color','b','Marker','.','MarkerSize',10);
    plot(time,xhat(:,i),'LineWidth',2,'Color','r','Marker','.','MarkerSize',10);
    set(gca,'FontSize',20,'FontName','Times')
    ylabel(ylabelz{i})
    
    figure(2);
    subplot(2,2,i)
    grid on
    hold on
    plot(time,x(:,i)-xhat(:,i),'LineWidth',2,'Color','b','Marker','.','MarkerSize',10);
    set(gca,'FontSize',20,'FontName','Times')
    ylabel(ylabelz{i})
end
figure(1)
legend('Actual State','Estimated State');

% Simulate on the Simulink model

sim('pendulum_openloop_Luenberger')

% The nonzero steady-state estimation error is because the actual system 
... is nonlinear. The Luenberger observer, however, is designed for the
... linearized version of the system. The system, when initiated with a
... non-zero initial condition, operates around a point (pendulum down) 
... that is away from the point (pendulum up), about which the linearization  
... is done. This causes the linearized model to be inaccurate. This, then,
... causes a discrepancy between the estimated dynamics and the actual 
... dynamics.
                            

%% (b) - Reduced-order observer (without delay)

% Cd = [1 0 0 0; 0 0 1 0] - already gives the first and third states
% Select T1 such that T=[Cd; T1] is full rank:

T1 = [0 1 0 0;
      0 0 0 1];
  
Tm = [Cd; T1];

% Create Abar, Bbar, and Cbar matrices:

Abar = Tm*Ad*inv(Tm);
Bbar = Tm*Bd;
Cbar = Cd*inv(Tm);

% Number of measured states = p = 2
p = size(Cd,1);
n = size(Ad,1);

% Since (Ad,Cd) is observable (see the solution of part (a) above), so is
... (Abar22,Abar12)

Abar11 = Abar(1:p,1:p);
Abar12 = Abar(1:p,p+1:end);
Abar21 = Abar(p+1:end,1:p);
Abar22 = Abar(p+1:end,p+1:end);

Bbar1 = Bbar(1:p,:);
Bbar2 = Bbar(p+1:end,:);

% Design observer
P = [exp(-9.9*T) exp(-10*T)];
L = place(Abar22',Abar12',P)';    % = [3.6289 -0.0748; 2.6256 6.7797] 

% Check that the eigenvalues are placed correctly
eig(Abar22-L*Abar12)           % = [0.3716 0.3679]' 
                                ... = [exp(-9.9*T) exp(-10*T)]'


% Simulate with the linearized and discretized system (Ad,Bd,Cd,Dd)
sysd = ss(Ad,Bd,Cd,Dd,T);
obsd = ss(Abar22-L*Abar12,[Bbar2-L*Bbar1 L Abar21-L*Abar11],eye(2),[],T);

time = 0:T:2;
u = zeros(size(time));
x0 = [0 0 1e-6 0]';
what0 = [0 1e-1]';

%%%%% With for loop iteration

% x = zeros(4,length(time)+1);
% y = zeros(2,length(time)+1);
% what = zeros(2,length(time)+1);
% x(:,1) = x0;
% what(:,1) = what0;
% y(:,1) = Cd*x0;
% 
% for k = 1:length(time)
%     
%     x(:,k+1) = sysd.A*x(:,k) + sysd.B*u(k);
%     y(:,k+1) = sysd.C*x(:,k+1);
%     
%     what(:,k+1) = obsd.A*what(:,k) + obsd.B*([u(k); y(:,k+1); y(:,k)]);
%     
% end
% 
% x = x(:,1:end-1)';
% xhat = (inv(Tm)*[y(:,1:end-1); what(:,1:end-1)])';

%%%%% With lsim

[y,t,x] = lsim(sysd,u,time,x0);

[~,t,what] = lsim(obsd,[u; [y(2:end,:)' zeros(size(y,2),1)]; y'],time,what0);
xhat = [y what]*inv(Tm)';

% Draw Plots
ylabelz = {'Linear Position','Linear Velocity','Angular Position','Angular Velocity'};
figure(3);
figure(4);
for i = 1:4
   
    figure(3);
    subplot(2,2,i)
    grid on
    hold on
    plot(time,x(:,i),'LineWidth',2,'Color','b','Marker','.','MarkerSize',10);
    plot(time,xhat(:,i),'LineWidth',2,'Color','r','Marker','.','MarkerSize',10);
    set(gca,'FontSize',20,'FontName','Times')
    ylabel(ylabelz{i})
    
    figure(4);
    subplot(2,2,i)
    grid on
    hold on
    plot(time,x(:,i)-xhat(:,i),'LineWidth',2,'Color','b','Marker','.','MarkerSize',10);
    set(gca,'FontSize',20,'FontName','Times')
    ylabel(ylabelz{i})
end
figure(3)
legend('Actual State','Estimated State');

% Simulate on the Simulink model

sim('pendulum_openloop_reducedorder')

% The nonzero steady-state estimation error in the second and third states
... is again because the actual system is nonlinear. The observer, however, 
... is designed for the linearized version of the system. The system, when 
... initiated with a non-zero initial condition, operates around a point 
... (pendulum down) that is away from the point (pendulum up), about which 
... the linearization is done. This causes the linearized model to be 
... inaccurate. This, then, causes a discrepancy between the estimated  
... dynamics and the actual dynamics. Moreover, observe that the estimation 
... of the first and third states - which are directly measured - is 
... not literal, i.e., they are NOT estimated, but instead measured. 
... Therefore, their "estimation error" is zero for all times.