clc;        %clear console
clear;      %clear workspace
close all;  %close all figures
pkg load control %remove if using MATLAB

%real system
Cf    = 10e-3;    %mF
Lf    = 6e-3;     %mH
A = [0 1/Cf;      %system matrix
     -1/Lf 0];
B = [0;1/Lf];     %input matrix
C = [1 0];        %output matrix
D = [0];          %feedthrough matrix

%task 1d)
%Remark: small imaginary part added to make place function work
poles = [-100+1e-6i, -100-1e-6i];                 %% <= change here to tune observer speed
M = place(A',C',poles);
L = M';

%task 1e)
A_full = [A, zeros(2,2);
           L*C (A-L*C)];    %from task c)
x0 = [10 0 0 0];

[Y,T,X] = initial(ss(A_full,zeros(4,1),zeros(1,4),0),x0,.2,5e-5);
subplot(211);
plot(T,X(:,1),'r','LineWidth',2)
hold on;
plot(T,X(:,3),'b','LineWidth',2);
legend('voltage','voltage-est');

subplot(212);
plot(T,X(:,2),'r','LineWidth',2)
hold on;
plot(T,X(:,4),'b','LineWidth',2);
legend('current','current-est');


%task 1f)
%system assumed by observer
Cf_obs  = 10e-3;        %mF
Lf_obs  = 5e-3;         %mH
A_obs_ol = [0 1/Cf_obs;    %system matrix observer open loop
     -1/Lf_obs 0];
B_obs = [0;1/Lf_obs];   %input matrix
C = [1 0];              %output matrix
D = [0];                %feedthrough matrix

L = place(A_obs_ol',C',poles)';
A_full2 = [A_obs_ol zeros(2,2) ; L*C A-L*C];

figure
[Y,T,X] = initial(ss(A_full2,zeros(4,1),zeros(1,4),0),x0,.2,5e-5);
subplot(211);
plot(T,X(:,1),'r','LineWidth',2)
hold on;
plot(T,X(:,3),'b','LineWidth',2);
legend('voltage','voltage-est');

subplot(212);
plot(T,X(:,2),'r','LineWidth',2)
hold on;
plot(T,X(:,4),'b','LineWidth',2);
legend('current','current-est');