% main_empc_pendel.m
% EMPC control of inverted pendelum example.
% DDIR, 15.03.2001
% Functions called: ss2h, q2qt.
clear all

imod=2;                        % imod=2->nonlinear pendlum model is used as process.
             
a21=14.493; b21=-1.4774;       % Linearized continuous pendlum model.
Ac=[0,1;a21 0]; Bc=[0;b21]; Dc=eye(2); Ec=[0;0];
dt=0.025;                      % Sampling interval.
[A,B,D,E]=c2dm(Ac,Bc,Dc,Ec,dt,'zoh'); % Discrete model.

L=100;                        % The prediction horizon.
% NOTE !!! L=11 gives a special result.
[H_L,O_L,OB]=ss2h(A,B,D,E,L,0); 
F_L=[OB H_L];

q=5*[1,0;0,1];                 % The weighting matrices.
Q=q2qt(q,L);
p=1;
P=eye(L)*p;

H=F_L'*Q*F_L+P;
O_LA=O_L*A; 
F_LQ=F_L'*Q;

[G_lq,R_lq]=dlqr(A,B,q,1);G_lq=-G_lq;  % LQ controller for comparison.

t=0:dt:5; N=length(t);          % Time intervall.
x0=[pi*15/180;0];                 % Initial value for the state.
r=zeros(L*2,1); fx=[0;0];
x=x0;
for i=1:N
   y=D*x;                       % Process measurement.
   
   p=O_LA*x;                    % Note: a lot of computing time can be spend
   f=F_LQ*(p-r);                % by doing this outside the for-loop.
   uf=-inv(H)*f; 
   u=uf(1);                     % Only 1st variable is used for control.
   %u=G_lq*x;                   % LQ control.
   Up(i,:)=u;
   
   U(i,:)=uf'; Y(i,:)=y';       % Store y and u.
   
   
   if imod == 1
      x=A*x+B*u;                % Update state equation (put u to the process).
   else
      fx(1)=x(2);               % Nonlinear pendlum model.
      fx(2)=a21*sin(x(1))+b21*u;
      x=x+dt*fx;
   end
        
end

figure(1)
subplot(211), plot(t,Y(:,1)), ylabel('q_1 [rad]'), grid
%title('En inverterte pendler paa vogn med EMPC-regulator (lineaer modell)')
title('Inverted pendulum on cart controlled with EMPC')
subplot(212), plot(t,Y(:,2)), ylabel('q_2=dot(q_1) [rad/s]'), grid
xlabel('Time [sec]')

figure(2)
plot(t,Up), grid
title('Control u(t): velocity of chart')

disp('EMPC controller')
Gt=-inv(H)*F_LQ*O_LA;
G_empc=Gt(1:1,:)
disp('LQ controller')
G_lq