// MPC con restricciones
clear
N = 50  // longitud del modelo
P = 5  // horizonte de predicción
M = 3  // horizonte de control
w = 0  // factor de ponderación sobre el control
dt = 1 // intervalo de discretización
tfinal = 30
t = linspace(0,tfinal,tfinal/dt+1)
kfinal = length(t)
r = ones(1,length(t))//;r(1:3) = 0;r(100:$) = 0.5
nv = 1 // nº de variables de estado

// restricciones
umin = -1.2
umax = 1.2
dumin = -0.2*ones(M,1)
dumax = 0.2*ones(M,1)

me = 0 // nº de restricciones de igualdad
CC = ones(M,M)
for i = 1:M-1
    CC(i,i+1:$) = 0
end
CC = -[CC; -CC]

// modelo
s = poly(0,'s')
g = 1/(5*s+1)
sl = syslin('c',g)
sld = dscr(sl,dt)
sldp = sld // asumo modelo perfecto: planta = modelo
PHI = sldp(2); GAMMA = sldp(3); C = sldp(4)
ti = linspace(0,N*dt,N+1)
si = csim('step',ti,sl)
//plot(ti,si,'o')
//title('coeficientes de respuesta a escalón unitario')
si = si(2:$); sN = si($)

// matriz dinámica Sf , PxM
    Sf(:,1) = si(1:P)'
for j = 2:M
    Sf(:,j) =[zeros(j-1,1); si(1:P-j+1)']
end

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

// matriz H de la función phi, MxM
H = Sf'*Sf + w*diag(ones(M,1))

// condiciones iniciales
uini = 0
xini = zeros(nv,1)
yini = 0
u = ones(min(P,kfinal),1)*uini
dup = zeros(N-2,1)
uP = zeros(P,1)
x(:,1) = xini
y(1) = yini
dist(1) = 0

//vector error, Px1
E = r(1:P)' - (Sp*dup + sN*uP + dist(1)*ones(P,1))

//vector p'
p = -E'*Sf

// restricciones
b = -[(umin - uini)*ones(M,1); (uini-umax)*ones(M,1)]

// cálculo del primer delta u
du = []
duk = qpsolve(H,p,CC,b,dumin,dumax,me)
du = [du duk(1)]
u(1) = uini + du(1)

// simulación
for k = 1:kfinal-1

    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
    
    if k-N+P > 0 then       uP = [uP(2:P);u(k-N+P)]
    end

    dist(k+1) = y(k+1) - ym(k+1) // perturbación 
    dup = [du(k); dup(1:N-3)]
        E = r(1:P)' - (Sp*dup + sN*uP + dist(k+1)*ones(P,1))

p = -E'*Sf

b = -[(umin - u(k))*ones(M,1); (u(k)-umax)*ones(M,1)]

    duk = qpsolve(H,p,CC,b,dumin,dumax,me)
    du = [du duk(1)]

    u(k+1) = u(k) + du(k+1) // nuevo nivel de control en el paso k

end

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

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