runquadprog.m

% Run quadprog optimizer
%   mpc       is the mpc configuration with the following fields:
%     A,B     System model matrices
%     Q       State cost
%     Qf      Terminal cost
%     R       Control signal cost
%     N       Cross-couping cost (may be [], which is typical in discrete design)
%     CX,Cx   State constraints
%     CU,Uc   Input constraints
%     Xf      Terminal constraints
%   x           is the current state estimate
%   xref        is the target state
%   N           is the prediction horizon
function [optx, fval, exitflag, output] = runquadprog(mpc, x, xref, Np, Nc)
  ns = size(mpc.A, 1); % Number of states
  ni = size(mpc.B, 2); % Number of inputs

  dx = x-xref;

  % Setup costs. f is linear costs which is not used.
  H = blkdiag(kron(eye(Np-1), mpc.Q), mpc.Qf, kron(eye(Np), mpc.R));
  f = zeros(length(H),1);

  if isfield(mpc, 'N') == 1 && isempty(mpc.N) == 0
    H(1:size(mpc.Q,1)*Np, size(mpc.Q,1)*Np+1:end) = kron(eye(Np), mpc.N);
    H(size(mpc.Q,1)*Np+1:end, 1:size(mpc.Q,1)*Np) = kron(eye(Np), mpc.N');
  end

  % Setup create constraint inequality
  A1 = kron(eye(Np), mpc.CX);
  A4 = kron(eye(Np), mpc.CU);
  A2 = zeros(size(A1,1), size(A4, 2));
  A3 = zeros(size(A4,1), size(A1, 2));
  A = [A1 A2; A3 A4];

  if isfield(mpc, 'Xf') == 0 || isempty(mpc.Xf)
    mpc.Xf = mpc.Cx - mpc.CX*xref;
  end
  b = [repmat(mpc.Cx - mpc.CX*xref, Np-1,1); mpc.Xf; repmat(mpc.Cu, Np, 1)];
  % Setup equality constraints (the model)
  Aeq1 = [zeros(ns,ns*Np); kron(eye(Np-1), -mpc.A) zeros(ns*(Np-1), ns)] + kron(eye(Np), eye(ns));
  Aeq2def = [eye(Nc, Nc) zeros(Nc, Np-Nc); zeros(Np-Nc, Nc-1) ones(Np-Nc, 1) zeros(Np-Nc, Np-Nc)];
  Aeq2 =  kron(Aeq2def, -mpc.B);
  Aeq = [Aeq1, Aeq2];
  beq_ = mpc.A * dx;
  beq = [beq_; zeros((Np-1)*ns, 1)];

  % Set optimizer options
  options = optimoptions('quadprog', 'Display', 'off', 'MaxIterations', 200, 'OptimalityTolerance', 1e-8);

  x0 = dx;
  [optx,fval,exitflag,output] = quadprog(H, f, A, b, Aeq, beq, [], [], x0, options);