QP Formulation¶

This page shows how the finite-horizon MPC problem in affine_mpc is converted into quadratic-program form. The derivation is written at the level of the mathematical structure used by the library, with the standard QP form that OSQP and DAQP accept.

Nomenclature¶

From Input Parameterization, the control points can be mapped to the input trajectory by

A custom basis takes the place of \(\Psi^p(\bm{\tau})\) in everything below, with the coefficients in place of the control points; it is dense, so the band of every sample holds every coefficient.

Quadratic Program (QP)¶

The form of a QP is

where \(\bm{z}\) is the design variable, \(P\) is the cost matrix, \(\bm{q}\) is the cost vector, \(A_\mathrm{qp}\) is the constraint matrix, and \(\bm{\ell}\) and \(\bm{\upsilon}\) are the lower and upper bounds on the constraints, respectively. Some of the symbols collide with MPC symbols, so we made slight modifications in notation from OSQP's documentation.

The design variable and the constraint rows are stacked from blocks, and only the blocks the options enable are present. The design variable holds the control points \(\bm{c}\), then the predicted states \(\bm{x}\) (sparse formulation only), then the slacks \(\bm{s}\) (soft state bounds only). The constraint rows are stacked in this order:

The MPC problem has summation terms in the cost function and a list of various constraints, which both need to be converted into a matrix multiply.

Sparse vs Condensed Design Variable¶

The main difference between two formulations is the choice of design vector \(\bm{z}\) used in the optimization. Sparse uses the control points and state trajectory while condensed only uses the control points:

With soft state bounds, both append the slacks \(\bm{s}\) (see Selected and Soft State Bounds).

The rest of this page describes how to convert the MPC optimization problem into a QP problem.

Cost Function in Matrix Form¶

The MPC cost is

This is the cost with the input cost on the control points (InputCostMode::ControlPoints); with Off, the input cost terms drop out of everything below, and the other cost terms are added in Optional Terms.

Since we're optimizing a norm, it doesn't matter which term is being subtracted. We will switch to use value minus reference in the QP conversion just to make the value positive, but it can be done either way.

First, we will create block diagonal matrices using the weights:

Now the cost function can be written as

We will use this form of the cost function as the starting point for both the sparse and condensed formulations, but the way we substitute in the model and convert to QP form will differ between the two.

Sparse Formulation¶

We'll start with the sparse formulation since it is perhaps a bit more intuitive. The dynamics remain as explicit linear equality constraints. The cost is then assembled directly in terms of \(\bm{x}\) and \(\bm{c}\), giving a larger but more structured QP.

The sparse may be preferable when:

• the horizon is long
• the state dimension is large
• state saturation is enabled

Model Constraint¶

Starting with the model

we need to rearrange it slightly to fit into a QP form. The design variable \(\bm{z}_{\mathrm{s}}\) has states and inputs (excluding \(\bm{x}_0\) since it is fixed and not optimized), so we group all of those on one side of the equation:

Now we'll convert to matrix form:

Substituting in the input parameterization gives

Finally, we can substitute in the design variable and get the model constraint:

Cost Terms¶

Starting with the cost function we derived above:

Here we see the last term has no design variables in it, so it is constant in terms of the optimization and can be dropped since we only care about the argmin. Scaling the cost by \(\frac{1}{2}\) also leaves the argmin unchanged and matches the QP form:

Now we have the definition of the QP cost terms:

Input Saturation¶

Bound the control points (InputBoundMode::ControlPoints, the default):

Bound the sampled inputs (InputBoundMode::Trajectory):

For degree \(p = 0\) these are the control point rows above, since the control points are the sampled inputs. For degree \(p = 1\), only the rows of \(\Psi^1(\bm{\tau})\) (and bounds) for the samples adjacent to each active knot are kept, which is exactly equivalent since the spline is linear between knots. A custom basis always uses these constraints, in place of bounding the control points, whatever input_bounds is, omitting the samples whose basis row norm is at most \(10^{-9}\) times the largest.

Bound the refined control points (InputBoundMode::Refined, degree \(p \ge 2\)):

The \(\eta_{\mathrm{r}} \times \eta\) matrix \(\Omega\) maps the control points of each input channel to the control points of the same spline on a refined knot vector (knot insertion), keeping only the \(\eta_{\mathrm{r}}\) refined control points that some sample depends on. It is banded (each row has at most \(p+1\) nonzeros) and depends only on the knots, so it is computed once at construction. For \(p \le 1\), Refined uses the ControlPoints (\(p = 0\)) or Trajectory (\(p = 1\)) rows, and for \(p \ge 2\) it uses the Trajectory rows when \(\eta_{\mathrm{r}} \ge T\). See Input Bounds. See Input Trajectory Saturation Constraint.

Slew Initial Input¶

Slew Rate¶

Bound the control point differences (SlewBoundMode::ControlPoints):

with \(\bm{\lambda} = [\lambda_1, \dots, \lambda_{\eta-1}]^\T\), \(\lambda_i = (\tau_{i+p} - \tau_i) / p\) (\(\lambda_i = 1\) for \(p = 0\)), and

is the first-difference matrix, so the row blocks of \((D_\eta \otimes I_m) \bm{c}\) are \(\bm{c}_i - \bm{c}_{i-1}\) for \(i = 1, \dots, \eta-1\). The factors depend only on the knots, so they are computed once at construction, and a limit change only updates the bounds; an infinite limit stays infinite, also where \(\lambda_i = 0\).

Bound the sampled changes (SlewBoundMode::Trajectory):

Here \(\bar{D}_T \in \R^{(T-1) \times T}\) is the first-difference matrix \(D_\eta\) with \(T\) in place of \(\eta\), so the row blocks are \(\bm{u}_k - \bm{u}_{k-1}\) for \(k = 1, \dots, T-1\). \(\bar{D}_T \Phi^p(\bm{\tau})\) is banded for a B-spline (each row spans the bands of two consecutive samples) and dense for a custom basis, whose rows of norm at most \(10^{-9}\) times the largest are omitted. For \(p = 0\), and for \(p = 1\) when every knot interval contains a whole sample step, Trajectory uses the ControlPoints rows, which are then exact.

State Saturation¶

Condensed Formulation¶

This formulation cannot use predicted states in its cost or constraint functions since they are not part of the design variable. Thus, the condensed formulation does not have the model as an explicit constraint function; instead, the model is implicitly wrapped into the cost function. This also means that state saturation cannot be directly performed on states in the condensed case.

The condensed formulation may be preferable when:

• the horizon is short
• the state dimension is small
• state bounds are not enabled

Model Constraint¶

To convert the model prediction into a single matrix multiply, we'll start writing out predicted states:

Writing this in matrix form yields

Now if we let

then the final condensed form is

This gives us a model function we can use to substitute out predicted states from the cost function. The condensed formulation does not have an explicit model constraint, so \(A_{\mathrm{model}}\), \(\bm{\ell}_{\mathrm{model}}\), and \(\bm{\upsilon}_{\mathrm{model}}\) are all empty.

Cost Terms¶

Starting with the cost function we derived above:

Substituting in the condensed model for the predicted state trajectory yields

and

Combining \(J_x\) and \(J_c\) back together and scaling by \(\frac{1}{2}\) (which, like dropping constants, leaves the argmin unchanged) gives the final form

where we see that

Input Saturation¶

Bound the control points (InputBoundMode::ControlPoints, the default):

Bound the sampled inputs (InputBoundMode::Trajectory):

For degree \(p = 0\) these are the control point rows above, since the control points are the sampled inputs. For degree \(p = 1\), only the rows of \(\Psi^1(\bm{\tau})\) (and bounds) for the samples adjacent to each active knot are kept, which is exactly equivalent since the spline is linear between knots. A custom basis always uses these constraints, in place of bounding the control points, whatever input_bounds is, omitting the samples whose basis row norm is at most \(10^{-9}\) times the largest.

Bound the refined control points (InputBoundMode::Refined, degree \(p \ge 2\)):

The \(\eta_{\mathrm{r}} \times \eta\) matrix \(\Omega\) maps the control points of each input channel to the control points of the same spline on a refined knot vector (knot insertion), keeping only the \(\eta_{\mathrm{r}}\) refined control points that some sample depends on. It is banded (each row has at most \(p+1\) nonzeros) and depends only on the knots, so it is computed once at construction. For \(p \le 1\), Refined uses the ControlPoints (\(p = 0\)) or Trajectory (\(p = 1\)) rows, and for \(p \ge 2\) it uses the Trajectory rows when \(\eta_{\mathrm{r}} \ge T\). See Input Bounds. See Input Trajectory Saturation Constraint.

Slew Initial Input¶

Slew Rate¶

Bound the control point differences (SlewBoundMode::ControlPoints):

Bound the sampled changes (SlewBoundMode::Trajectory):

where \(\bm{\lambda}\), the first-difference matrices \(D_\eta\) and \(\bar{D}_T\), and the choice of rows are as in the sparse formulation.

State Saturation¶

Optional Terms¶

The derivations above use the input cost on the control points and bound every state. The following sections show how the other options change the QP of both formulations.

Selected and Soft State Bounds¶

With bounded_states listing \(n_b\) of the \(n\) states, let \(E_b \in \R^{n_b \times n}\) hold the matching rows of \(I_n\). The state saturation rows become \((I_T \otimes E_b)\) times the state rows above, with bounds \(\mathbf{1}_T \otimes \bm{x}_{\mathrm{min}}\) and \(\mathbf{1}_T \otimes \bm{x}_{\mathrm{max}}\) of size \(n_b\) (and \(-(I_T \otimes E_b)\bm{v}\) in the condensed formulation): \(n_b T\) rows instead of \(nT\). In the condensed formulation these are the selected rows of \(S\), so no product is needed.

Soft bounds add one slack per bounded state, \(\bm{s} \in \R^{n_b}\), after the formulation's variables. A row can not hold one slack on both of its sides, so each limit becomes its own one-sided row:

which is \(2 n_b T + n_b\) rows. The penalty \(J_s = \bm{\rho}_1^\T \bm{s} + \bm{s}^\T \diag(\bm{\rho}_2) \bm{s}\) is scaled by \(\frac{1}{2}\) like the rest of the cost, so the slack block of \(P\) is \(\diag(\bm{\rho}_2)\) and the slack part of \(\bm{q}\) is \(\frac{1}{2} \bm{\rho}_1\).

Input Rate Cost¶

The input rate cost only involves the control points, which come first in both \(\bm{z}_{\mathrm{s}}\) and \(\bm{z}_{\mathrm{c}}\), so it adds the same terms to both formulations. With the \(T \times T\) difference matrix \(D_T = I_T - I_T^{(-1)}\), which unlike \(D_\eta\) keeps the first row so that \(\bm{u}_0 - \bm{u}_{-1}\) is included, and \(D = D_T \otimes I_m\), the input changes are

so with \(\bar{R}_\Delta = I_T \otimes R_\Delta\),

using \(D^\T (\bm{e}_1 \otimes R_\Delta \bm{u}_{-1}) = \bm{e}_1 \otimes R_\Delta \bm{u}_{-1}\). Scaling by \(\frac{1}{2}\) as before, the control point block of \(P\) gains \((D \Psi)^\T \bar{R}_\Delta (D \Psi)\) and the control point part of \(\bm{q}\) gains \(-\Psi^\T (\bm{e}_1 \otimes R_\Delta \bm{u}_{-1})\), which only involves the control points that shape \(\bm{u}_0\).

Since \(\Psi = \Phi \otimes I_m\), \((D \Psi)^\T \bar{R}_\Delta (D \Psi) = G \otimes R_\Delta\) with \(G = (D_T \Phi)^\T (D_T \Phi)\). \(G\) depends only on the basis (banded for B-splines, dense for a custom basis), so it is computed once; a weight change only rescales it. The previous input \(\bm{u}_{-1}\) changes after every solve, so \(\bm{q}\) is updated at every solve when this cost is enabled.

Input Cost on the Sampled Inputs¶

The derivations above use the input cost on the control points (InputCostMode::ControlPoints), whose block of \(P\) is \(\bar{R} = I_\eta \otimes R\). With InputCostMode::Trajectory, the input cost is evaluated on the sampled inputs \(\bm{u} = \Psi^p(\bm{\tau}) \, \bm{c}\) instead. Like the rate cost, it only involves the control points, so it adds the same terms to both formulations. With \(\bar{R}_T = I_T \otimes R\) and the stacked sampled reference \(\bm{u}_{\mathrm{ref}}\),

Scaling by \(\frac{1}{2}\) as before, the control point block of \(P\) gains \(\Psi^\T \bar{R}_T \Psi\) in place of \(\bar{R}\), and the control point part of \(\bm{q}\) gains \(-\Psi^\T \bar{R}_T \bm{u}_{\mathrm{ref}}\) in place of \(-\bar{R} \bm{c}_{\mathrm{ref}}\).

Since \(\Psi = \Phi \otimes I_m\), \(\Psi^\T \bar{R}_T \Psi = \Phi^\T \Phi \otimes R\). \(\Phi^\T \Phi\) depends only on the basis, so it is computed once and declared in the sparsity pattern at construction; a weight change only rescales it. For a B-spline of degree \(p\) it is banded with bandwidth \(2p + 1\) (diagonal for degree 0, where its entries are the block lengths of move-blocking), and it is dense for a custom basis. A reference change only updates \(\bm{q}\), accumulated sample by sample from the band of \(\Phi\), so it needs no matrix update or refactorization.

General and Terminal Constraints¶

The general constraints pair \(\bm{x}_k\) with \(\bm{u}_k\), while the stacked state \(\bm{x}\) starts at \(\bm{x}_1\), so the \(k = 0\) rows only hold the input part, with the measured state moved into the bounds:

With \(\bm{u} = \Psi^p(\bm{\tau})\, \bm{c}\), the input part of all \(n_h T\) rows is \((I_T \otimes H_u)\, \Psi^p(\bm{\tau})\, \bm{c}\): row block \(k\) holds \(H_u\) times the basis weights of \(\bm{u}_k\) on the control points of its band. The state part of the rows for \(k \geq 1\) is \(H_x\) applied to \(\bm{x}_1, \dots, \bm{x}_{T-1}\), which each formulation writes as it writes state rows:

• Sparse: \(H_x\) on the columns of \(\bm{x}_k\) in \(\bm{z}_{\mathrm{s}}\), with bounds \(\bm{h}_{\min}\) and \(\bm{h}_{\max}\).
• Condensed: \(H_x S_k\) on the control point columns, where \(S_k\) holds the rows of \(S\) for \(\bm{x}_k\), with bounds \(\bm{h}_{\min} - H_x \bm{v}_k\) and \(\bm{h}_{\max} - H_x \bm{v}_k\). Row \(i\) of \(H_x S_k\) is a sum over the nonzero entries of row \(i\) of \(H_x\), so a sparse \(H_x\) costs little, and only the columns of the control points up to the band of step \(k-1\) are nonzero.

The terminal rows are the same state rows for \(\bm{x}_T\) alone: \(H_{\mathrm{f}}\) on the columns of \(\bm{x}_T\) (sparse), or \(H_{\mathrm{f}} S_T\) with bounds shifted by \(-H_{\mathrm{f}} \bm{v}_T\) (condensed). State bounds on selected states are the special case \(H_u = 0\) with \(H_x = E_b\).

The general rows add \(n_h T\) rows and the terminal rows \(n_{h,\mathrm{f}}\), after every other block of \(A_{\mathrm{qp}}\). The general bounds depend on \(\bm{x}_0\) (through the \(k = 0\) rows in both formulations, and through \(\bm{v}\) in the condensed one), and so do the condensed terminal bounds (through \(\bm{v}\)), so they are updated at every solve.

Accessing the QP¶

getQP() returns the QP as last written by solve(), prepare(), or assembleQP(), and assembleQP(x0, qp) assembles it for the current settings and an initial state without solving, before or after initializeSolver(). Both give a QuadraticProgram with \(P\) and \(A_{\mathrm{qp}}\) (P and A) as compressed sparse column matrices in the same form for both formulations (scipy.sparse.csc_matrix in Python, which needs pip install "affine-mpc[sparse]"), \(P\) full symmetric, and the vectors \(\bm{q}\), \(\bm{\ell}\), \(\bm{\upsilon}\) (q, l, u). The cost omits the constant terms of the MPC cost, equality rows have \(\bm{\ell} = \bm{\upsilon}\), and unbounded sides are \(\pm\infty\). Neither function is meant for the control loop: both copy the QP, and assembleQP() leaves its changes to be sent again by the next prepare() or solve().

getQPVariableBlocks() and getQPConstraintBlocks() name the blocks of \(\bm{z}\) and of the rows of \(A_{\mathrm{qp}}\), in order, each with its offset and size; only the blocks the options enable are listed.

For example, to hand the QP to another solver or to count its rows and nonzeros:

qp = affine_mpc.QuadraticProgram()
mpc.assembleQP(x0, qp)  # or qp = mpc.getQP() after a solve
print(qp.P.nnz, qp.A.nnz)
for block in mpc.getQPConstraintBlocks():
    print(block.name, block.offset, block.size)

affine_mpc::QuadraticProgram qp;
mpc.assembleQP(x0, qp); // or auto qp = mpc.getQP(); after a solve
std::cout << qp.P.nonZeros() << " " << qp.A.nonZeros() << "\n";
for (const affine_mpc::QPBlock& block : mpc.getQPConstraintBlocks())
  std::cout << block.name << " " << block.offset << " " << block.size << "\n";

getDualSolution() gives the dual solution \(\bm{y}\) of the last solve, one value per row of \(A_{\mathrm{qp}}\) in the order of getQPConstraintBlocks(). It satisfies \(P\bm{z} + \bm{q} + A_{\mathrm{qp}}^\T\bm{y} = 0\) at a solution, so \(y_i > 0\) when row \(i\) is at its upper bound, \(y_i < 0\) at its lower bound, and \(y_i = 0\) when it is inactive; for example, the nonzero entries of the input_bounds block show where the input saturates. These are the multipliers of the QP's cost, which is half the MPC cost. With OSQP they are approximate, like its primal solution, and SolveInfo reports how closely the solution satisfies the optimality conditions: primal_residual is the largest violation of \(\bm{\ell} \le A_{\mathrm{qp}}\bm{z} \le \bm{\upsilon}\), and dual_residual the largest entry of \(|P\bm{z} + \bm{q} + A_{\mathrm{qp}}^\T\bm{y}|\). They are the solver's own values, so they cost nothing to read in the control loop, while computing them from getQP() copies the QP. They are absolute, so compare them with a tolerance that suits your problem; OSQP accepts a solution once each is below eps_abs plus eps_rel times a problem-dependent scale. DAQP does not report residuals (they are NaN), since its active-set solutions meet its tolerances.

Implementation Mapping¶

At a high level, the derivation maps to the implementation as follows:

• Parameterization defines the evaluation map \(\Psi^p(\bm{\tau})\)
• Formulation::Condensed (Formulation.Condensed in Python) uses the condensed relation between \(\bm{c}\) and \(\bm{x}\)
• Formulation::Sparse keeps \(\bm{x}\) directly in the QP decision vector
• MPC manages the model, references, weights, constraints, and solver lifecycle the same way for both

The final mathematical ingredient needed for robust usage is understanding what may be updated after initialization and why OSQP imposes fixed sparsity constraints. That is covered in Runtime Updates.