close all
A = [-1 1.5;1 -2];
B = [1;0];
C = [1 0];
D = 0;
x_initial = [-0.5;-1];
e_initial = x_initial;    %means that estimated x has an initial value of [0;0]


% open loop estimator
%----------------------

% augmented matrices

A_ol = [A zeros(size(A));zeros(size(A)) A];
B_ol = [B;zeros(size(B))];
C_ol = [C zeros(size(C))];
D_ol = D;
sys_ol = ss(A_ol,B_ol,C_ol,D_ol);

% create an input
t = 0:0.01:10;

% u = [0; ones((length(t)-1),1)];
% u = [0; ones((length(t)-1)/2,1); -1*ones((length(t)-1)/2,1)];
u = [0; ones((length(t)-1)/4,1); -1*ones((length(t)-1)/4,1) ;ones((length(t)-1)/4,1); -1*ones((length(t)-1)/4,1)];

[y_ol,t,x_ol]=lsim(sys_ol,u,t,[x_initial;e_initial]);
    
figure
plot(t,x_ol(:,1:2),'LineWidth',2)
hold on
plot(t,x_ol(:,1:2)-x_ol(:,3:4),'--','LineWidth',2)
title('true state variables and estimated state variables versus time for open loop estimator')
legend({'$x_1$','$x_2$','$\hat{x_1}$','$\hat{x_2}$'},'Interpreter','Latex')
xlabel('time (sec)')
ylabel('state variable')


figure
plot(t,x_ol(:,3:4),'LineWidth',2)
title('estimation error for open loop estimator')
ylim([-1 1])
legend('e_1','e_2')
xlabel('time (sec)')
ylabel('estimation error')



% closed loop estimator
%------------------------

desiredEstimatorPoles = [-3 -4];  % the more negative the faster the convergence of the estimator
L = place(A.',C.',desiredEstimatorPoles).';  % find the feedback gain matrix

% construct agumented matrices of sys+estimator error


A_cl = [A zeros(size(A));zeros(size(A)) A-L*C];
B_cl = [B;zeros(size(B))];
C_cl = [C zeros(size(C))];
D_cl = D;
sys_cl = ss(A_cl,B_cl,C_cl,D_cl);

[y_cl,t,x_cl]=lsim(sys_cl,u,t,[x_initial;e_initial]);
    
figure
plot(t,x_cl(:,1:2),'LineWidth',2)
hold on
plot(t,x_cl(:,1:2)-x_cl(:,3:4),'--','LineWidth',2)
title('true state variables and estimated state variables versus time for closed loop estimator')
legend({'$x_1$','$x_2$','$\hat{x_1}$','$\hat{x_2}$'},'Interpreter','Latex')
xlabel('time (sec)')
ylabel('state variable')



figure
plot(t,x_cl(:,3:4),'LineWidth',2)
title('estimation error for closed loop estimator')
ylim([-1 1])
legend('e_1','e_2')
xlabel('time (sec)')
ylabel('estimation error')
