Per Skarin committed Sep 08, 2020 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 ``````% 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); ``````