% main_clopid_ex1.m
% Test SID of closed loop system. Generate data matrices (U,Y) by simulating
% a SISO one state system with a PID controller.
% Use DSr and DSR_e to estimate models and predict outputs.

A=0.9; B=0.5; D=1; K=0.6;   % The underlying system, a SISO n=1 system.
N=2000;                     % Using N samples

Kp=0.5; Ti=10;              % PID controller parameters
R=prbs1(N,100,350);         % The reference signal

X=zeros(N,1); e=zeros(N,1); % allocate space for storing variables
U=zeros(N,1);  Y=zeros(N,1);

% Noise
sw=0.05; sv=0.07;           % Standard deviations for v_k and w_k, respectively
w=randn(N,1)*sw;  v=randn(N,1)*sv;

% Main simulation loop
x=0; z=R(1);
for k=1:N
    y=D*x+w(k);   % The measurement

    e=R(k)-y;     % Control deviation

    u=z+Kp*e;     % Simple PI controller
    z=z+Kp*e/Ti;

    % Store variables
    U(k,1)=u; Y(k,1)=y; X(k,1)=x;

    x=A*x+B*u+v(k);     % Put control action to plant, update model

end

% Plotting
figure(1)
plot(Y)

%Identify models: Known data matrices U and Y
n=1; L=5; J=5;
[a_dsr,b_dsr,d_dsr,e_dsr,c_dsr,f_dsr,x0_dsr]=dsr(Y,U,L,0,J);
K_dsr=c_dsr*pinv(f_dsr);
[a_dsre,b_dsre,d_dsre,e_dsre,K_dsre,F_dsre,x0_dsre]=dsr_e(Y,U,L,0,J,n);

%Simulate predictions
Y_dsr=dsrsim(a_dsr,b_dsr,d_dsr,e_dsr,U,x0_dsr);
Y_dsre=dsrsim(a_dsre,b_dsre,d_dsre,e_dsre,U,x0_dsre);

figure(2)
subplot(211), plot([R,Y,Y_dsr])
subplot(212), plot([R,Y,Y_dsre])

figure(3)
subplot(211), dstep(a_dsr,b_dsr,d_dsr,e_dsr)
subplot(212), dstep(a_dsre,b_dsr,d_dsre,e_dsre)

% Minor differences with resuts from DSR and DSR_e
