// MPC
clear
N = 50  // longitud del modelo
P = 25  // horizonte de predicción
M = 1  // horizonte de control
w = 0  // factor de ponderación sobre el control
dt = 0.1 // intervalo de discretización
t = 0:dt:6
kfinal = length(t)
r = ones(1,length(t))
nv = 2 // nº de variables de estado

// modelo
A = [-2.4048 0 ; 0.83333 -2.2381]; B = [7 ; -1.117]; C = [0 1]; D = [0]
sl = syslin('c',A,B,C,D)
sld = dscr(sl,dt)
sldp = sld // asumo modelo perfecto: planta = modelo
PHI = sld(2); GAMMA = sld(3); C = sld(4)
si = csim('step',t,sl)
plot(t,si,'o')
title('coeficientes de respuesta a escalón unitario')
si = si(2:$); sN = si($)

function [Sf,Sp,K1] = matgen(si,P,M,N,w)
// matriz dinámica Sf
for j = 1:M
    Sf(:,j) =[zeros(j-1,1); si(1:P-j+1)']
end

// matriz pasado Sp
for i = 1:P
    Sp(i,:) = [si(i+1:N-1) zeros(1,i-1)]
end

// matriz de ganancia de control K
K = inv(Sf'*Sf + w*eye(M))*Sf'
K1 = K(1,:)
endfunction

function deltau = deltacalc(Sp,K1,sN,dup,d,r,u,k)
 //   [M,P] =size(K)
    uP = zeros(P,1)
    for i = 1:P
        if k-N+i > 0
            uP(i) = u(k-N+i)
        else
            uP(i) = 0
        end
    end

    E = r*ones(P,1) - (Sp*dup +  sN*uP + d*ones(P,1))
    deltau = K1*E
endfunction

// condiciones iniciales
uini = 0
xini = zeros(nv,1)
yini = 0
u = ones(min(P,kfinal),1)*uini
dup = zeros(N-2,1)
x(:,1) = xini
y(1) = yini
dist(1) = 0
[Sf,Sp,K1] = matgen(si,P,M,N,w)

// simulación
for k = 1:kfinal
    du(k) = deltacalc(Sp,K1,sN,dup,dist(k),r(k),u,k) // cálculo del cambio en el control
    
    if k > 1
        u(k) = u(k-1) + du(k) // nuevo nivel de control en el paso k
    else
        u(k) = uini + du(k)
    end
    
    x(:,k+1) = PHI*x(:,k) + GAMMA*u(k) // modelo discreto de planta
    y(k+1)   = C*x(:,k+1)
    
    if k-N+1 > 0
    ym(k+1) = si(1)*du(k) + Sp(1,:)*dup + sN*u(k-N+1) // predicción
    else
    ym(k+1) = si(1)*du(k) + Sp(1,:)*dup
    end
    
    dist(k+1) = y(k+1) - ym(k+1) // perturbación
    dup = [du(k); dup(1:N-3)]
end

figure(2)
subplot(211)
plot([-1 t],[0 y(1:$-1)']);plot([-1 0 t],[0 0 r],'g--')
xlabel('t');ylabel('y')

subplot(212)
plot2d2([-1 t],[0 u'])
xlabel('t');ylabel('u')