We consider an n-DOF rigid manipulator with joint coordinates \( q \in \mathbb{R}^n \) and joint velocities \( \dot{q} \in \mathbb{R}^n \). From robotics courses you already know the standard manipulator equations

\[ \mathbf{M}(q)\ddot{q} + \mathbf{C}(q,\dot{q})\dot{q} + \mathbf{g}(q) = \mathbf{B}\,\tau, \]

where \( \mathbf{M}(q) \) is the inertia matrix, \( \mathbf{C}(q,\dot{q}) \) collects Coriolis/centrifugal terms, \( \mathbf{g}(q) \) is gravity, and \( \mathbf{B} \) selects actuated joints. The control input is \( \tau \in \mathbb{R}^{m} \) (joint torques).

For MPC we use a state-space representation. A natural choice of state is \( x = \begin{bmatrix} q \\ \dot{q} \end{bmatrix} \in \mathbb{R}^{2n} \). Around a nominal tracking trajectory \( (q^{\mathrm{ref}}(t), \dot{q}^{\mathrm{ref}}(t), \tau^{\mathrm{ref}}(t)) \), we linearize the dynamics as in previous chapters:

\[ \dot{x}(t) = \mathbf{A}(t)\,x(t) + \mathbf{B}(t)\,u(t) + \mathbf{c}(t), \]

where \( u =\tau - \tau^{\mathrm{ref}} \) is the incremental control input and the affine term \( \mathbf{c}(t) \) accounts for linearization errors and the reference trajectory dynamics.

For digital control with sampling period \( T_s \), we discretize the linearized system (e.g. zero-order hold) to obtain

\[ x_{k+1} = \mathbf{A}_k x_k + \mathbf{B}_k u_k + \mathbf{d}_k, \]

where \( x_k \approx x(kT_s) \), \( u_k \approx u(kT_s) \) and \( \mathbf{d}_k \) is the discrete-time affine term. For many problems we can treat \( \mathbf{A}_k, \mathbf{B}_k \) as constant over the horizon (time-invariant approximation) when the tracking trajectory changes slowly.

The measured output of interest (tracking variable) is modeled as

\[ y_k = \mathbf{C}_k x_k + \mathbf{e}_k, \]

where \( y_k \) could be:

• Joint-space tracking: \( y_k = q_k \), so \( \mathbf{C}_k = \begin{bmatrix} \mathbf{I} & \mathbf{0} \end{bmatrix} \).
• Task-space tracking: \( y_k = h(q_k) \in \mathbb{R}^p \), with \( h(\cdot) \) the forward kinematics. Linearizing around \( q_k^{\mathrm{ref}} \) yields \( y_k \approx y_k^{\mathrm{ref}} + \mathbf{J}_k(q_k - q_k^{\mathrm{ref}}) \), so \( \mathbf{C}_k = \begin{bmatrix} \mathbf{J}_k & \mathbf{0} \end{bmatrix} \).

flowchart TD
  R["Reference trajectory (q_ref, dq_ref, tau_ref)"] --> L["Linearize dynamics around ref"]
  L --> D["Discretize with Ts"]
  D --> M["Prediction model x_{k+1} = A x_k + B u_k + d_k"]
  M --> O["Output model y_k = C x_k + e_k"]
  O --> MPC["Finite-horizon MPC tracking problem"]
        

This discrete-time, possibly time-varying, state-space model is the foundation for MPC. The next step is to build an N-step prediction of states and outputs over a finite horizon.

Fix a prediction horizon length \( N \). Given the current state estimate \( x_k \), we predict future states and outputs under a sequence of future inputs \( u_k, \dots, u_{k+N-1} \). For simplicity assume time-invariant \( \mathbf{A}, \mathbf{B}, \mathbf{C} \) and affine term \( \mathbf{d} \).

Recursively applying the dynamics:

\[ \begin{aligned} x_{k+1} &= \mathbf{A}x_k + \mathbf{B}u_k + \mathbf{d},\\ x_{k+2} &= \mathbf{A}x_{k+1} + \mathbf{B}u_{k+1} + \mathbf{d} = \mathbf{A}^2 x_k + \mathbf{A}\mathbf{B}u_k + \mathbf{B}u_{k+1} + (\mathbf{A} + \mathbf{I})\mathbf{d}, \end{aligned} \]

and so on, we obtain for any step \( \ell = 1,\dots,N \):

\[ x_{k+\ell} = \mathbf{A}^{\ell} x_k + \sum_{j=0}^{\ell-1} \mathbf{A}^{\ell-1-j}\mathbf{B} u_{k+j} + \sum_{j=0}^{\ell-1} \mathbf{A}^{\ell-1-j} \mathbf{d}. \]

We now stack future states and outputs into big vectors:

\[ \mathbf{X} = \begin{bmatrix} x_{k+1} \\ x_{k+2} \\ \vdots \\ x_{k+N} \end{bmatrix} ,\quad \mathbf{U} = \begin{bmatrix} u_k \\ u_{k+1} \\ \vdots \\ u_{k+N-1} \end{bmatrix} ,\quad \mathbf{Y} = \begin{bmatrix} y_{k+1} \\ y_{k+2} \\ \vdots \\ y_{k+N} \end{bmatrix}. \]

Using the recursion, there exist matrices \( \mathbf{S}_x, \mathbf{S}_u, \mathbf{S}_d \) such that

\[ \mathbf{X} = \mathbf{S}_x x_k + \mathbf{S}_u \mathbf{U} + \mathbf{S}_d, \]

and since \( y_{k+\ell} = \mathbf{C} x_{k+\ell} \), we also obtain

\[ \mathbf{Y} = \mathbf{C}_\mathrm{blk}\,\mathbf{X} = \mathbf{C}_\mathrm{blk}\mathbf{S}_x x_k + \mathbf{C}_\mathrm{blk}\mathbf{S}_u \mathbf{U} + \mathbf{C}_\mathrm{blk}\mathbf{S}_d, \]

where \( \mathbf{C}_\mathrm{blk} = \mathrm{diag}(\mathbf{C},\dots,\mathbf{C}) \) is a block-diagonal matrix repeating \( \mathbf{C} \) along the horizon.

Denoting the stacked reference outputs over the horizon by \( \mathbf{Y}^{\mathrm{ref}} \), we define the output tracking error

\[ \mathbf{E}_y = \mathbf{Y} - \mathbf{Y}^{\mathrm{ref}} = \underbrace{\mathbf{C}_\mathrm{blk}\mathbf{S}_x x_k + \mathbf{C}_\mathrm{blk}\mathbf{S}_d - \mathbf{Y}^{\mathrm{ref}}}_{\mathbf{b}} + \underbrace{\mathbf{C}_\mathrm{blk}\mathbf{S}_u}_{\mathbf{T}} \mathbf{U} = \mathbf{b} + \mathbf{T}\mathbf{U}. \]

Here \( \mathbf{b} \) is the known part (given current state and references), and \( \mathbf{T} \) maps the stacked control sequence \( \mathbf{U} \) to the stacked output error.

flowchart TD
  X0["Current state x_k"] --> Sx["Build S_x, S_u, S_d from (A,B,d)"]
  Ref["Reference outputs over horizon Y_ref"] --> Err["Compute b = C_blk S_x x_k + C_blk S_d - Y_ref"]
  Sx --> Pred["Y = b + T U where T = C_blk S_u"]
  Err --> Pred
  Pred --> Cost["Quadratic cost J(U) from E_y and U"]
  Cost --> QP["Condensed QP for MPC tracking"]
        

We now define a quadratic cost that penalizes output tracking error and control effort. For each step \( j = 1,\dots,N \):

\[ \ell_j = (y_{k+j} - y_{k+j}^{\mathrm{ref}})^\top \mathbf{Q}_y (y_{k+j} - y_{k+j}^{\mathrm{ref}}) + u_{k+j-1}^\top \mathbf{R} u_{k+j-1}, \]

where \( \mathbf{Q}_y \succeq 0 \) and \( \mathbf{R} \succ 0 \). We may also include a terminal cost \( V_f(x_{k+N}) = (x_{k+N} - x_{k+N}^{\mathrm{ref}})^\top \mathbf{P} (x_{k+N} - x_{k+N}^{\mathrm{ref}}) \), where \( \mathbf{P} \) comes from a Riccati equation (as in finite-horizon LQR).

Stacking over the horizon with block-diagonal matrices \( \mathbf{Q} = \mathrm{diag}(\mathbf{Q}_y,\dots,\mathbf{Q}_y) \) and \( \mathbf{R}_\mathrm{blk} = \mathrm{diag}(\mathbf{R},\dots,\mathbf{R}) \), we can write

\[ J(\mathbf{U}) = \mathbf{E}_y^\top \mathbf{Q}\,\mathbf{E}_y + \mathbf{U}^\top \mathbf{R}_\mathrm{blk} \mathbf{U} + V_f(x_{k+N}). \]

Using \( \mathbf{E}_y = \mathbf{b} + \mathbf{T}\mathbf{U} \), we expand

\[ \begin{aligned} J(\mathbf{U}) &= (\mathbf{b} + \mathbf{T}\mathbf{U})^\top \mathbf{Q} (\mathbf{b} + \mathbf{T}\mathbf{U}) + \mathbf{U}^\top \mathbf{R}_\mathrm{blk} \mathbf{U} + V_f(x_{k+N}) \\ &= \mathbf{U}^\top (\mathbf{T}^\top \mathbf{Q}\mathbf{T} + \mathbf{R}_\mathrm{blk}) \mathbf{U} + 2\,\mathbf{b}^\top \mathbf{Q}\mathbf{T}\mathbf{U} + \mathbf{b}^\top \mathbf{Q}\mathbf{b} + V_f(x_{k+N}). \end{aligned} \]

Introducing the condensed quadratic form

\[ \mathbf{H} = 2(\mathbf{T}^\top \mathbf{Q}\mathbf{T} + \mathbf{R}_\mathrm{blk}), \quad \mathbf{f} = 2\,\mathbf{T}^\top \mathbf{Q}^\top \mathbf{b}, \]

and ignoring the constant term (which does not affect the optimizer), the optimization problem can be written as

\[ \min_{\mathbf{U}} \; \frac{1}{2}\mathbf{U}^\top \mathbf{H}\mathbf{U} + \mathbf{f}^\top \mathbf{U}. \]

In this unconstrained case, the optimal control sequence is given by the unique minimizer

\[ \mathbf{U}^\star = -\mathbf{H}^{-1}\mathbf{f}, \]

provided \( \mathbf{H} \succ 0 \). This yields a finite-horizon LQR-type feedback law when we recast it in terms of the current state \( x_k \). The MPC controller then applies only the first component \( u_k^\star \) and repeats at the next sampling step.

Positive definiteness of \( \mathbf{H} \). Since \( \mathbf{R}_\mathrm{blk} \succ 0 \), we have for any nonzero \( \mathbf{U} \):

\[ \mathbf{U}^\top \mathbf{H} \mathbf{U} = 2\mathbf{U}^\top (\mathbf{T}^\top \mathbf{Q}\mathbf{T} + \mathbf{R}_\mathrm{blk})\mathbf{U} \ge 2\mathbf{U}^\top \mathbf{R}_\mathrm{blk}\mathbf{U} > 0, \]

hence \( \mathbf{H} \succ 0 \) and the minimizer is unique.

In practice we will add constraints (joint limits, torque limits, etc.), which turn this into a constrained QP. That is the main topic of the next lesson; here we focus on the unconstrained quadratic structure.

The above derivation is generic; the main difference between joint-space and task-space MPC lies in the choice of the output mapping \( y_k \) and the associated weighting matrix \( \mathbf{Q}_y \).

4.1 Joint-Space Tracking

If the tracking variable is the joint angle vector itself, \( y_k = q_k \in \mathbb{R}^n \), then \( \mathbf{C} = [\mathbf{I} \;\; \mathbf{0}] \) and \( \mathbf{Q}_y \) directly weights joint errors. This is natural for low-level servo control, where reference trajectories are given in joint coordinates.

\[ y_k - y_k^{\mathrm{ref}} = q_k - q_k^{\mathrm{ref}}. \]

The stacked reference \( \mathbf{Y}^{\mathrm{ref}} \) is built from the desired joint trajectories \( q_{k+1}^{\mathrm{ref}},\dots,q_{k+N}^{\mathrm{ref}} \).

4.2 Task-Space Tracking

For end-effector trajectories (position and/or orientation), \( y_k = h(q_k) \in \mathbb{R}^p \), where \( h \) is the forward kinematics. Linearization around the reference trajectory yields

\[ y_k - y_k^{\mathrm{ref}} \approx \mathbf{J}_k (q_k - q_k^{\mathrm{ref}}), \]

with \( \mathbf{J}_k \) the geometric Jacobian evaluated at \( q_k^{\mathrm{ref}} \). In state coordinates,

\[ y_k - y_k^{\mathrm{ref}} \approx \begin{bmatrix} \mathbf{J}_k & \mathbf{0} \end{bmatrix} (x_k - x_k^{\mathrm{ref}}). \]

This fits directly into the generic output model with \( \mathbf{C}_k = [\mathbf{J}_k \;\; \mathbf{0}] \). The rest of the MPC formulation is unchanged; only the matrices \( \mathbf{C}_k \), \( \mathbf{C}_\mathrm{blk} \), and \( \mathbf{Q}_y \) differ. This allows a unified implementation handling both control spaces.

We now show small prototype implementations that:

1. Construct the condensed matrices \( \mathbf{S}_x, \mathbf{S}_u \) and \( \mathbf{T} \).
2. Build the Hessian \( \mathbf{H} \) and gradient \( \mathbf{f} \).
3. Solve the unconstrained quadratic program and extract the first input \( u_k^\star \).

For simplicity we consider a fixed linear model \( x_{k+1} = \mathbf{A}x_k + \mathbf{B}u_k \) and joint-space tracking \( y_k = \mathbf{C}x_k \).

5.1 Python (NumPy + simple linear algebra)

import numpy as np

# Dimensions
nx = 4   # state dimension
nu = 2   # input dimension
ny = 2   # output dimension

# Example linearized model (replace with your robot's linearization)
A = np.eye(nx) + 0.01 * np.random.randn(nx, nx)
B = 0.01 * np.random.randn(nx, nu)
C = np.hstack((np.eye(ny), np.zeros((ny, nx - ny))))

N = 10  # horizon length

# Weights
Qy = np.eye(ny)
R = 0.01 * np.eye(nu)

# Build block-diagonal Q and R_blk
Q_blk = np.kron(np.eye(N), Qy)
R_blk = np.kron(np.eye(N), R)

def build_prediction_matrices(A, B, N):
    nx, nu = A.shape[0], B.shape[1]
    Sx = np.zeros((N * nx, nx))
    Su = np.zeros((N * nx, N * nu))

    A_power = np.eye(nx)
    for i in range(N):
        A_power = A_power @ A  # A^(i+1)
        Sx[i*nx:(i+1)*nx, :] = A_power

        for j in range(i+1):
            A_pow = np.linalg.matrix_power(A, i - j)
            Su[i*nx:(i+1)*nx, j*nu:(j+1)*nu] = A_pow @ B
    return Sx, Su

Sx, Su = build_prediction_matrices(A, B, N)
C_blk = np.kron(np.eye(N), C)

T = C_blk @ Su  # maps U to stacked output error

def mpc_step(xk, y_ref_seq):
    """
    xk: current state (nx,)
    y_ref_seq: stacked reference outputs over horizon (N * ny,)
    """
    # Build b = C_blk Sx xk - Y_ref
    b = C_blk @ (Sx @ xk) - y_ref_seq

    H = 2 * (T.T @ Q_blk @ T + R_blk)
    f = 2 * (T.T @ Q_blk @ b)

    # Unconstrained optimum: U* = -H^{-1} f
    U_star = -np.linalg.solve(H, f)

    u0_star = U_star[:nu]
    return u0_star, U_star

# Example usage
xk = np.zeros(nx)
y_ref_seq = np.zeros(N * ny)  # track zero output
u0_star, U_star = mpc_step(xk, y_ref_seq)
print("MPC control at time k:", u0_star)
      

5.2 C++ (Eigen-based prototype)

In C++ we typically use Eigen for linear algebra and connect it to an external QP solver (e.g. qpOASES, OSQP). Below we implement the unconstrained solution with Eigen only.

#include <iostream>
#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;

int main() {
    const int nx = 4;
    const int nu = 2;
    const int ny = 2;
    const int N  = 10;

    MatrixXd A = MatrixXd::Identity(nx, nx);
    A += 0.01 * MatrixXd::Random(nx, nx);
    MatrixXd B = 0.01 * MatrixXd::Random(nx, nu);
    MatrixXd C(ny, nx);
    C << 1,0,0,0,
          0,1,0,0;

    // Build prediction matrices
    MatrixXd Sx = MatrixXd::Zero(N * nx, nx);
    MatrixXd Su = MatrixXd::Zero(N * nx, N * nu);

    MatrixXd A_power = MatrixXd::Identity(nx, nx);
    for (int i = 0; i < N; ++i) {
        A_power = A_power * A; // A^(i+1)
        Sx.block(i*nx, 0, nx, nx) = A_power;

        for (int j = 0; j <= i; ++j) {
            MatrixXd A_pow = MatrixXd::Identity(nx, nx);
            for (int p = 0; p < (i - j); ++p) {
                A_pow = A_pow * A;
            }
            Su.block(i*nx, j*nu, nx, nu) = A_pow * B;
        }
    }

    // Block-diagonal C and weights
    MatrixXd C_blk = MatrixXd::Zero(N * ny, N * nx);
    for (int i = 0; i < N; ++i) {
        C_blk.block(i*ny, i*nx, ny, nx) = C;
    }

    MatrixXd Qy = MatrixXd::Identity(ny, ny);
    MatrixXd R  = 0.01 * MatrixXd::Identity(nu, nu);

    MatrixXd Q_blk = MatrixXd::Zero(N * ny, N * ny);
    MatrixXd R_blk = MatrixXd::Zero(N * nu, N * nu);
    for (int i = 0; i < N; ++i) {
        Q_blk.block(i*ny, i*ny, ny, ny) = Qy;
        R_blk.block(i*nu, i*nu, nu, nu) = R;
    }

    MatrixXd T = C_blk * Su;

    // One MPC step
    VectorXd xk = VectorXd::Zero(nx);
    VectorXd y_ref = VectorXd::Zero(N * ny); // zero reference

    VectorXd b = C_blk * (Sx * xk) - y_ref;

    MatrixXd H = 2.0 * (T.transpose() * Q_blk * T + R_blk);
    VectorXd f = 2.0 * (T.transpose() * Q_blk * b);

    // Solve H U = -f (unconstrained case)
    VectorXd U_star = H.ldlt().solve(-f);
    VectorXd u0_star = U_star.head(nu);

    std::cout << "u0* = " << u0_star.transpose() << std::endl;
    return 0;
}
      

5.3 Java (using a generic QP solver, e.g., ojAlgo)

In Java we can use linear algebra and optimization libraries such as ojAlgo. The snippet below sketches how to set up the unconstrained quadratic cost (only the core logic is shown).

import org.ojalgo.matrix.Primitive64Matrix;
import org.ojalgo.optimisation.QuadraticSolver;
import org.ojalgo.optimisation.Optimisation;

public class MPCJava {
    public static void main(String[] args) {
        int nx = 4, nu = 2, ny = 2, N = 10;

        // Example A, B, C (you would build them as in Python/C++)
        double[][] Adata = new double[nx][nx];
        double[][] Bdata = new double[nx][nu];
        double[][] Cdata = new double[ny][nx];
        // ... fill Adata, Bdata, Cdata ...

        Primitive64Matrix A = Primitive64Matrix.FACTORY.rows(Adata);
        Primitive64Matrix B = Primitive64Matrix.FACTORY.rows(Bdata);
        Primitive64Matrix C = Primitive64Matrix.FACTORY.rows(Cdata);

        // Build Sx, Su, C_blk, Q_blk, R_blk, etc. (not shown for brevity)
        // Suppose we already have H and f as Primitive64Matrix:
        Primitive64Matrix H = /* 2(T' Q T + R_blk) */;
        Primitive64Matrix f = /* 2 T' Q b */;

        QuadraticSolver.Builder builder = QuadraticSolver.getBuilder();
        builder.quadratic(H);
        builder.linear(f);
        // No constraints set => unconstrained QP

        Optimisation.Result result = builder.build().solve();
        Primitive64Matrix Ustar = Primitive64Matrix.FACTORY.columns(result);

        // Extract first control move
        double[] u0 = new double[nu];
        for (int i = 0; i < nu; ++i) {
            u0[i] = Ustar.get(i, 0);
        }
        System.out.println("u0* = ");
        for (int i = 0; i < nu; ++i) {
            System.out.print(u0[i] + " ");
        }
    }
}
      

5.4 MATLAB / Simulink

MATLAB is particularly convenient for prototyping MPC. We can build the condensed matrices and use quadprog (unconstrained case is solved analytically but we keep the QP call for later extension with constraints).

nx = 4; nu = 2; ny = 2; N = 10;

A = eye(nx) + 0.01 * randn(nx);
B = 0.01 * randn(nx, nu);
C = [eye(ny), zeros(ny, nx - ny)];

Qy = eye(ny);
R  = 0.01 * eye(nu);

Q_blk = kron(eye(N), Qy);
R_blk = kron(eye(N), R);

% Build prediction matrices Sx, Su
Sx = zeros(N*nx, nx);
Su = zeros(N*nx, N*nu);
A_power = eye(nx);
for i = 1:N
    A_power = A_power * A;
    Sx((i-1)*nx+1:i*nx, :) = A_power;
    for j = 1:i
        A_pow = A^(i-j);
        Su((i-1)*nx+1:i*nx, (j-1)*nu+1:j*nu) = A_pow * B;
    end
end

C_blk = kron(eye(N), C);
T = C_blk * Su;

xk = zeros(nx, 1);
y_ref = zeros(N*ny, 1);

b = C_blk * (Sx * xk) - y_ref;

H = 2 * (T' * Q_blk * T + R_blk);
f = 2 * (T' * Q_blk * b);

% Unconstrained solution: U* = -H\f
U_star = -H \ f;
u0_star = U_star(1:nu);
disp('u0* = ');
disp(u0_star);

% Simulink hint:
%  - Implement the A,B,C model as a State-Space block.
%  - Implement the MPC computation in a MATLAB Function block,
%    which takes x_k, y_ref as inputs and outputs u_k* each sample.
      

5.5 Wolfram Mathematica

In Mathematica we can directly express the matrices and use NMinimize or LinearSolve for the unconstrained quadratic case.

nx = 4; nu = 2; ny = 2; N = 10;

(* Example numeric matrices (replace with actual linearization) *)
A = IdentityMatrix[nx] + 0.01 RandomReal[{-1, 1}, {nx, nx}];
B = 0.01 RandomReal[{-1, 1}, {nx, nu}];
C = Join[IdentityMatrix[ny], ConstantArray[0, {ny, nx - ny}], 2];

Qy = IdentityMatrix[ny];
R  = 0.01 IdentityMatrix[nu];

Qblk = KroneckerProduct[IdentityMatrix[N], Qy];
Rblk = KroneckerProduct[IdentityMatrix[N], R];

(* Build Sx, Su *)
Sx = ConstantArray[0, {N*nx, nx}];
Su = ConstantArray[0, {N*nx, N*nu}];

Apower = IdentityMatrix[nx];
Do[
  Apower = Apower.A;
  Sx[[ (i - 1)*nx + 1 ;; i*nx, ;; ]] = Apower;
  Do[
    Apow = MatrixPower[A, i - j];
    Su[[ (i - 1)*nx + 1 ;; i*nx, (j - 1)*nu + 1 ;; j*nu ]] = Apow.B;
    {j, 1, i}
  ];
  ,
  {i, 1, N}
];

Cblk = KroneckerProduct[IdentityMatrix[N], C];
T = Cblk.Su;

xk = ConstantArray[0., nx];
yref = ConstantArray[0., N*ny];

b = Cblk.(Sx.xk) - yref;

H = 2 (Transpose[T].Qblk.T + Rblk);
f = 2 Transpose[T].Qblk.b;

(* Unconstrained optimum U* = -Inverse[H].f *)
Ustar = -LinearSolve[H, f];
u0star = Take[Ustar, nu]
      

Problem 1 (Derivation of the Stacked State Prediction): Consider the time-invariant linear system \( x_{k+1} = \mathbf{A}x_k + \mathbf{B}u_k \) and a fixed horizon \( N \). Derive an explicit formula for \( x_{k+\ell} \) in terms of \( x_k \) and the inputs \( u_k,\dots,u_{k+\ell-1} \), and show how this leads to the stacked representation \( \mathbf{X} = \mathbf{S}_x x_k + \mathbf{S}_u \mathbf{U} \).

Solution:

For \( \ell = 1 \), we have \( x_{k+1} = \mathbf{A} x_k + \mathbf{B}u_k \). For \( \ell = 2 \),

\[ x_{k+2} = \mathbf{A}x_{k+1} + \mathbf{B}u_{k+1} = \mathbf{A}(\mathbf{A}x_k + \mathbf{B}u_k) + \mathbf{B}u_{k+1} = \mathbf{A}^2 x_k + \mathbf{A}\mathbf{B}u_k + \mathbf{B}u_{k+1}. \]

Proceeding by induction, assume that for some \( \ell \ge 1 \), \[ x_{k+\ell} = \mathbf{A}^{\ell} x_k + \sum_{j=0}^{\ell-1} \mathbf{A}^{\ell-1-j}\mathbf{B}u_{k+j}. \] Then

\[ \begin{aligned} x_{k+\ell+1} &= \mathbf{A}x_{k+\ell} + \mathbf{B}u_{k+\ell} \\ &= \mathbf{A}\mathbf{A}^{\ell}x_k + \mathbf{A} \sum_{j=0}^{\ell-1} \mathbf{A}^{\ell-1-j}\mathbf{B}u_{k+j} + \mathbf{B}u_{k+\ell} \\ &= \mathbf{A}^{\ell+1}x_k + \sum_{j=0}^{\ell-1} \mathbf{A}^{\ell-j}\mathbf{B}u_{k+j} + \mathbf{B}u_{k+\ell} \\ &= \mathbf{A}^{\ell+1}x_k + \sum_{j=0}^{\ell} \mathbf{A}^{\ell-j}\mathbf{B}u_{k+j}, \end{aligned} \]

which completes the induction. Stacking \( x_{k+1},\dots,x_{k+N} \) into \( \mathbf{X} \) and arranging the coefficients of the inputs into blocks gives the matrices \( \mathbf{S}_x \) and \( \mathbf{S}_u \).

Problem 2 (Positive Definiteness of the MPC Hessian): In the unconstrained tracking MPC formulation with cost \( J(\mathbf{U}) = \mathbf{E}_y^\top \mathbf{Q}\,\mathbf{E}_y + \mathbf{U}^\top \mathbf{R}_\mathrm{blk}\mathbf{U} \), assume \( \mathbf{Q} \succeq 0 \) and \( \mathbf{R}_\mathrm{blk} \succ 0 \). Prove that the condensed Hessian \( \mathbf{H} = 2(\mathbf{T}^\top \mathbf{Q}\mathbf{T} + \mathbf{R}_\mathrm{blk}) \) is positive definite.

Solution:

For any nonzero \( \mathbf{U} \in \mathbb{R}^{N n_u} \),

\[ \begin{aligned} \mathbf{U}^\top \mathbf{H} \mathbf{U} &= 2\mathbf{U}^\top (\mathbf{T}^\top \mathbf{Q}\mathbf{T} + \mathbf{R}_\mathrm{blk}) \mathbf{U} \\ &= 2\mathbf{U}^\top \mathbf{T}^\top \mathbf{Q}\mathbf{T}\mathbf{U} + 2\mathbf{U}^\top \mathbf{R}_\mathrm{blk}\mathbf{U}. \end{aligned} \]

The first term is nonnegative since \( \mathbf{Q} \succeq 0 \). The second term is strictly positive because \( \mathbf{R}_\mathrm{blk} \succ 0 \), hence \( \mathbf{U}^\top \mathbf{R}_\mathrm{blk}\mathbf{U} > 0 \) for all nonzero \( \mathbf{U} \). Therefore \( \mathbf{U}^\top \mathbf{H}\mathbf{U} > 0 \) for all nonzero \( \mathbf{U} \), implying \( \mathbf{H} \succ 0 \).

Problem 3 (Joint-Space vs Task-Space Output Matrices): For a 3-DOF manipulator, suppose we track the planar position of the end-effector \( y = [p_x, p_y]^\top \) in task space. The joint coordinates are \( q \in \mathbb{R}^3 \). Show how to construct the matrix \( \mathbf{C}_k \) in the state-space model \( y_k = \mathbf{C}_k x_k \) using the Jacobian, and explain under what assumptions this linearization is justified.

Solution:

The forward kinematics map is \( h: \mathbb{R}^3 \to \mathbb{R}^2 \), with \( y = h(q) \). Linearizing around the reference joint configuration \( q_k^{\mathrm{ref}} \) we have

\[ h(q_k) \approx h(q_k^{\mathrm{ref}}) + \mathbf{J}_k (q_k - q_k^{\mathrm{ref}}), \]

where \( \mathbf{J}_k = \frac{\partial h}{\partial q}\big|_{q_k^{\mathrm{ref}}} \in \mathbb{R}^{2\times 3} \) is the Jacobian. If the state is \( x_k = [q_k^\top, \dot{q}_k^\top]^\top \), then

\[ y_k - y_k^{\mathrm{ref}} \approx \mathbf{J}_k (q_k - q_k^{\mathrm{ref}}) = \begin{bmatrix} \mathbf{J}_k & \mathbf{0}_{2\times 3} \end{bmatrix} (x_k - x_k^{\mathrm{ref}}). \]

Hence we can set \( \mathbf{C}_k = [\mathbf{J}_k \;\; \mathbf{0}] \). The linearization is justified if the deviation \( q_k - q_k^{\mathrm{ref}} \) is small enough so that higher-order terms in the Taylor expansion are negligible over the prediction horizon. This is typically enforced by sufficiently fast sampling and well-designed references.

Problem 4 (Connection to Finite-Horizon LQR): Consider the unconstrained MPC formulation with quadratic cost and linear dynamics. Show that if the reference is zero (regulation problem) and the horizon \( N \) tends to infinity, the MPC law approaches the infinite-horizon LQR feedback law.

Solution (sketch):

For zero reference, the cost is \( J = \sum_{j=0}^{N-1} x_{k+j}^\top \mathbf{Q} x_{k+j} + u_{k+j}^\top \mathbf{R} u_{k+j} \). The dynamic programming solution leads to the Riccati recursion \( \mathbf{P}_{j} = \mathbf{Q} + \mathbf{A}^\top \mathbf{P}_{j+1}\mathbf{A} - \mathbf{A}^\top \mathbf{P}_{j+1}\mathbf{B} (\mathbf{R} + \mathbf{B}^\top \mathbf{P}_{j+1}\mathbf{B})^{-1} \mathbf{B}^\top \mathbf{P}_{j+1}\mathbf{A} \) with terminal condition \( \mathbf{P}_N = \mathbf{P}_f \).

The optimal control at step \( k+j \) is \( u_{k+j}^\star = -\mathbf{K}_j x_{k+j} \) with \( \mathbf{K}_j = (\mathbf{R} + \mathbf{B}^\top \mathbf{P}_{j+1}\mathbf{B})^{-1} \mathbf{B}^\top \mathbf{P}_{j+1}\mathbf{A} \). As \( N \to \infty \), under standard stabilizability assumptions, \( \mathbf{P}_j \) converges to the unique stabilizing solution \( \mathbf{P}_\infty \) of the algebraic Riccati equation, and \( \mathbf{K}_j \) converges to the constant LQR feedback gain \( \mathbf{K}_\infty \). The receding-horizon MPC law thus approaches the infinite-horizon LQR law \( u_k = -\mathbf{K}_\infty x_k \).

Problem 5 (Dimension Counting): Suppose \( x_k \in \mathbb{R}^{8} \), \( u_k \in \mathbb{R}^{4} \), and we choose horizon \( N = 15 \). What are the dimensions of \( \mathbf{X}, \mathbf{U}, \mathbf{Y}, \mathbf{S}_x, \mathbf{S}_u \) in the stacked formulation if \( y_k \in \mathbb{R}^{6} \)?

Solution:

• \( \mathbf{X} \in \mathbb{R}^{N n_x} = \mathbb{R}^{15 \cdot 8} = \mathbb{R}^{120} \).
• \( \mathbf{U} \in \mathbb{R}^{N n_u} = \mathbb{R}^{15 \cdot 4} = \mathbb{R}^{60} \).
• \( \mathbf{Y} \in \mathbb{R}^{N n_y} = \mathbb{R}^{15 \cdot 6} = \mathbb{R}^{90} \).
• \( \mathbf{S}_x \in \mathbb{R}^{N n_x \times n_x} = \mathbb{R}^{120 \times 8} \).
• \( \mathbf{S}_u \in \mathbb{R}^{N n_x \times N n_u} = \mathbb{R}^{120 \times 60} \).

In this lesson we translated the robot's nonlinear dynamics into a discrete-time prediction model suitable for MPC, linearized around a reference trajectory. We stacked the predicted states and outputs over a finite horizon, derived the matrices \( \mathbf{S}_x, \mathbf{S}_u \), and formed the output tracking error as an affine function of the stacked input sequence \( \mathbf{U} \). This led to a condensed quadratic cost with Hessian \( \mathbf{H} = 2(\mathbf{T}^\top \mathbf{Q}\mathbf{T} + \mathbf{R}_\mathrm{blk}) \) and gradient \( \mathbf{f} = 2\mathbf{T}^\top \mathbf{Q}\mathbf{b} \).

We emphasized the distinction between joint-space and task-space tracking as different choices of the output mapping \( \mathbf{C}_k \), and demonstrated how the same MPC machinery applies to both. Finally, we presented prototype implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica, illustrating how to build prediction matrices and solve the resulting unconstrained quadratic program. In the next lesson, we will enrich this formulation with hard constraints on states and inputs, leading to constrained MPC for robots.