clc
clear all
close all

 s=tf('s');

 aux=open('state-space.mat');
sys_ss=aux.sys;

%Open loop plant dynamics
Ap=sys_ss.a;
Bp=sys_ss.b(:,1);
Cm2=sys_ss.c(1:2,:);
Cm=sys_ss.c(1,:);
Cr=sys_ss.c(1,:);
Dp=sys_ss.d(1:1);

I=eye(size(Ap));
% G=minreal(sys_ss.c(1,:)*inv(s*I-sys_ss.a)*sys_ss.b);



nx=length(Ap);
ny=size(Cm,1);
nu=size(Bp,2);

%augmenting the system to get integral action
Aaug=[Ap zeros(nx,1); -Cr  zeros()];
Baug=[Bp;0];
Bcmd=[zeros(1,nx) 1]';
%Assuming that P_bottom and P_top are measured or estimated elsewhere
Caug=[[Cm zeros(ny,1)]; [zeros(1,nx) 1]];
Daug=[Dp;0];



%% step 1: design a LQR controller to obtain desired robustness properties
      
    %Tuning constant for the reference model: higher is faster
    q_i=12;
    Q=diag([0 0  0 0 1]);
    Q(end)=q_i*Q(end);
    R=1*eye(1,1);
    
    Klqr=lqr(Aaug,Baug,Q,R);
    I=eye(size(Aaug));
    
    Llqr=minreal(Klqr*inv(s*I-Aaug)*Baug);
    
    %reference system
    Aref=Aaug-Baug*Klqr;
    Bref=[zeros(1,nx) 1]';
    Cref=Caug;
      
    Ccl=[Cref; %ym
        -Klqr   %ucontrol
        -Klqr*Aref %udot
        ];
    
    Mcl=ss(Aref,Bref,Ccl,0);
    [Gm,Pm,Wgm,Wpm] = margin(Llqr);
    auxmargin=allmargin(Llqr);
    [yall,t]=step(Mcl);
    y=yall(:,1);
    u=yall(:,end-1);
    udot=yall(:,end);

    figure(1)
    plot(t,y);
    ylabel('dP, Kpa')
    xlabel('Time, s')
    
    figure(2)
    plot(t,u);
    ylabel('Input, %')
    xlabel('Time, s')
    
  
%%



%reference system
Aref=Aaug-Baug*Klqr;
Bref=[zeros(1,nx) 1]';
Cref=Caug;

%reference model
Mcl=ss(Aref,Bref,Cref,0);



%% observer design -squaring up stage
% zeroplace=  [-0.1  -1 -1]; %only used for pole placement place() 
% Bsqr=getAugSquareUp(Aaug,Baug,[Caug ;1 1 0 0 1],zeroplace,'lqr');
%  Bsqr=10+20*rand(5,2);
Bsqr=5e-4*[21.9498   17.1921
   16.7062   21.1664
   15.9845   24.8509
   19.0519   18.4867
   18.4529   18.5871
];

sys_aug=ss(Aaug,Bsqr,Caug,0);
display('-----zeros original coordinates----')
zervec = tzero(minreal(sys_aug))
if zervec>0
    error('transmission zeros must be stable')
end

%%Compute Kalman gain
Q0=Q(1:end,1:end);
R0=2*eye(size(Caug,1));

% vvec=logspace(-2,1.5,10);
v=100;

Qv=Q0+(v+1)/v*Bsqr*(Bsqr');

Rv=v/(v+1)*R0;

%========using ricatti equation======
[Lv,Pv] = lqe(Aaug,eye(size(Aaug)),Caug,Qv,Rv);

% LTR Controller
Ac = Aref-Lv*Caug;
Bc1 = Lv;
Bc2=Bref;
Cc = -Klqr;
Dc1 = [0. 0. ];
Dc2 = 0.;

%SS model of loop gain at the plant input
Ain = [ Aaug 0.*Baug*Cc;  Bc1*Caug Ac];
Bin = [ Baug; Bc1*Daug];
Cin = -[ Dc1*Caug Cc];%change sign for loop gain
Din = -[ Dc1*Daug];

Lltr = minreal(ss(Ain,Bin,Cin,Din));
wrange=logspace(-5,2,200);
figure
bode(Llqr,Lltr,wrange)
drawnow
grid on
hold on
legend('LQR','LTR')



% write closed loop observer LTI model
Aob=Aref-Lv*Caug;
Bob=[Bref Lv];
%outputs estimation error: ey=Cxest-ym
Cob=[Caug];
Dob=[zeros(ny+1,1)   -eye(ny+1)];
sysobserver=ss(Aob,Bob,Cob,Dob);
sysobserver.InputName={'ref','P1','Inte'};
sysobserver.OutputName={'e1','e3'};

syscontrol=ss(Aob,Bob,-Klqr,0);

%Closed loop reference model eigenvalues
eig(Aob)
                                                           

%constants for the adaptive law
S=[1 0];
auxM=Baug'*Caug'*R0^(-0.5);
[U,Sig,V]=svd(auxM);
W=(U*V)';
F=R0^(-0.5)*W*S';

save('adaptive_system4States', 'Klqr', 'sysobserver','F','syscontrol','Aref','Bref','Cref');


%% Simulate controller against Linear model
 AdaptOn=1;
 callLinearSim

%% Simulate controller against Esmaeils NonLinear model
AdaptOn=1;
callNonLinearSim