% main_dlq_rdu2
% Purpose: Simulate LQ optimal tracking problem with weighting of Delta u_k.
% Using a receeding horizon strategy.
% Functions called: prbs1, dric_solv
% Author: David Di Ruscio, october 2000.
clear
A = [1.5,1.0,0.1;-0.7,0.0,0.1;0,0,0.85];    % Process model.
B = [0.0 0.0;0.0 1.0;1.0 0.0];
D = [3.0,0.0,-0.6;0.0,1.0 1.0];
Q = eye(2)*0.03;                            % Weighting matrix for r_k-y_k.
Rw=eye(2);                                  % Weighting for Delta u_k=u_k-u_{k-1}.
S=Q;                                        % Weighting for final deviation.

Ns=250;                                     % Simulation horizon.
rand('seed',0), randn('seed',0)
ref=[0.9*ones(Ns,1)+0.1*prbs1(Ns,30,70) ...
     0.4*ones(Ns,1)+0.1*prbs1(Ns,30,70)];   % Reference r_k.

N=10;                                       % Prediction horizon.
%------- A FAIRLY GENERAL IMPLEMENTATION ------------------------------------
[n,r]=size(B); [m,n]=size(D);               %Augmented model matrices.
At=[A B;zeros(r,n) eye(r,r)];
Bt=[B;eye(r,r)];
Dt=[D zeros(m,r)];
Qt=zeros(n+r,n+r); Qt(1:n,1:n)=D'*Q*D;      % Same as Qt=Dt*Q*Dt.
St=Dt'*S*Dt;

[G,R]=dric_solv(At,Bt,St,Qt,Rw,Ns);         % Solve algebraic Riccati equation.
G1=G(:,1:n); G2=G(:,n+1:n+r);               % Note: what happens if you put
G3=-inv(Rw+Bt'*R*Bt)*Bt';                   % Ns=N ?

Hd=ddcgain(A,B,D,zeros(m,r));               % Compute steady state initial values
r0=ref(1,:)';                               % x0 and u0 for specified y0=r0. 
u0=inv(Hd)*r0;
x0=pinv(eye(n)-A)*B*u0;

x=x0; u_old=u0;                             % Initialize before simulation.
Hd_h=-inv(eye(n+r)-(At+Bt*G)')*Dt'*Q;


for k=1:Ns-N                                % Simulate system with optimal control.
   y=D*x;                                   % Measurement.
   
   ref1=ref(k:k+N-1,:);                     % Using N future references.
   h=Hd_h*ref1(N,:)';                       % Final value condition for h_k.                                                  
   %h=-Dt'*S*ref1(N,:)';                    % Alternative final value.
   for i=N:-1:2                             % Compute feed-forward signal, h_k.
       h=(At+Bt*G)'*h-Dt'*Q*ref1(i,:)';
   end     
   du=G1*x+G2*u_old+G3*h;                   % Optimal control.
   u=du+u_old;
   u_old=u;
   
   Y(k,:)=y'; U(k,:)=u';                    % save outputs and inputs.
 
   x=A*x+B*u;                               % Put control to the system.
end

t=1:Ns-N;                                   % Plot results.
figure(1), clf
plot(t,[ref(1:Ns-N,:) Y]), grid, title('Output y_k and reference r_k')
text(110,1.05,'y_1')
text(110,0.55,'y_2')
xlabel('Discrete time')
figure(2), clf
plot(t,U), grid, title('Optimal control u_k')
text(130,0.13,'u_1')
text(130,-0.06,'u_2')
xlabel('Discrete time')
% END MAIN_DLQ_RDU2
