% Demo
% demo1_advanced_control.m
% Optimal control with integral action of random systems.
% Using dlqdu_pi.m and drss.m
% 07.03.2024

figure(1), clf
clear all

im=3;
% Random n'th order system
if im==1
  n=3; ny=1; nu=1;
elseif im==2
    n=3; ny=2; nu=2;
elseif im==3         % System with n=6 and nu=3 inputs and ny=3 outputs
    n=6; ny=3; nu=3;
end
sys=drss(n,ny,nu);
A=sys.a; B=sys.b; D=sys.c;

% Set time interval
dt=0.001;
t=0:dt:1;
N=length(t);

% Reference signal
Tmin=50; Tmax=150;
if im==1
    R=prbs1(N,Tmin,Tmax); r=R(1);
elseif im==2
    R=[prbs1(N,Tmin,Tmax) prbs1(N,Tmin+20,Tmax+10)]; r=R(1,:)';
elseif im==3
    R=[prbs1(N,Tmin,Tmax) prbs1(N,Tmin+20,Tmax+10) prbs1(N,50,150)]; r=R(1,:)';
end
Y=zeros(N,ny); X=zeros(N,n);

% Calculate system gain and set som initial parameters
Hd=ddcgain(A,B,D,0);
u0=pinv(Hd)*r; x0=pinv(eye(n)-A)*B*u0;

P=eye(nu); Q=10*eye(ny);
[G1,G2,At,Bt,Dt,Rr]=dlqdu_pi(A,B,D,Q,P);

x=x0; u=u0;
y_old=D*x; x_old=x;
for k=1:N
    y=D*x;

    r=R(k,:)';
    u=u+G1*(x-x_old)+G2*(y_old-r);

    Y(k,:)=y'; X(k,:)=x';
    U(k,:)=u';
    y_old=y; x_old=x;
   
    x=A*x+B*u;

end

figure(1)
if im==1
    plot(t,[R(:,1) Y(:,1)])
elseif im==2
    subplot(211), plot(t,[R(:,1) Y(:,1)]), title('Output y_1 and reference r_1')
    subplot(212), plot(t,[R(:,2) Y(:,2)]), title('Output y_2 and reference r_2')
    xlabel('Time ')
elseif im==3
    subplot(311), plot(t,[R(:,1) Y(:,1)]), title('Output y_1 and reference r_1')
    subplot(312), plot(t,[R(:,2) Y(:,2)]), title('Output y_2 and reference r_2')
    subplot(313), plot(t,[R(:,3) Y(:,3)]), title('Output y_3 and reference r_3')
    xlabel('Time ')
end

% System Identification: Using DSR
L=3; J=L;
[A,B,D,E,C,F,x0]=dsr(Y,U,L,0,J,1,n);   % Identify DSRmodel, n'th order
Ym=dsrsim(A,B,D,E,U,x0);               % Simulate model
er=Y-Ym; V=norm(er'*er/N)
