In previous chapters you have seen state-space models for robots, linearization around trajectories, and finite-horizon optimal control (e.g. LQR). Model Predictive Control (MPC) takes these ingredients and packages them into a receding-horizon feedback scheme:

• At each sampling instant \(k\), measure (or estimate) the state \( x_k \) of the robot.
• Use a predictive model to compute an optimal open-loop input sequence over a finite horizon \(N\).
• Apply only the first control action, discard the remaining ones.
• Advance time to \(k+1\) and repeat with the new state.

Consider a discrete-time linear time-invariant (LTI) model obtained from linearizing the robot dynamics (details in Section 2):

\[ x_{k+1} = A x_k + B u_k,\quad y_k = C x_k,\quad x_k \in \mathbb{R}^n,\; u_k \in \mathbb{R}^m \]

A classical finite-horizon optimal control problem chooses a sequence \( u_k, u_{k+1},\dots,u_{k+N-1} \) that minimizes a cost functional. In MPC, we repeatedly solve such a problem, always starting from the current state. This transforms a sequence of open-loop problems into a closed-loop controller.

Intuitively, the horizon acts like a moving window into the future: we plan ahead, execute the first step, then shift the window forward and re-plan based on up-to-date information and measurements.

For an \(n\)-DOF rigid manipulator, the continuous-time joint-space dynamics are (from your robotics background):

\[ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau,\quad q \in \mathbb{R}^n,\; \tau \in \mathbb{R}^n \]

We define a state vector combining joint positions and velocities:

\[ x(t) = \begin{bmatrix} q(t) \\[2mm] \dot{q}(t) \end{bmatrix},\quad u(t) = \tau(t) \]

Around a nominal trajectory \(\big(q^{\star}(t),\dot{q}^{\star}(t),\tau^{\star}(t)\big)\) we linearize the dynamics (see earlier chapters) to obtain a continuous-time linear system

\[ \dot{x}(t) = A_c x(t) + B_c u(t) \]

After choosing a sampling time \(T_s\) and applying zero-order hold (ZOH) on the input, we obtain a discrete-time model suitable for MPC:

\[ x_{k+1} = A x_k + B u_k,\quad A = e^{A_c T_s},\quad B = \int_{0}^{T_s} e^{A_c s} B_c\,\mathrm{d}s \]

In many cases (e.g. for a single joint modeled as a double integrator), this reduces to simple matrices. For a 1-DOF joint with position \(q\) and velocity \(\dot{q}\), driven by torque \(\tau\), the (approximate) discrete-time model is

\[ x_k = \begin{bmatrix} q_k \\ \dot{q}_k \end{bmatrix},\quad A = \begin{bmatrix} 1 & T_s \\ 0 & 1 \end{bmatrix},\quad B = \begin{bmatrix} \tfrac{T_s^2}{2 I} \\ \tfrac{T_s}{I} \end{bmatrix} I \tau_k \]

where \(I\) is the scalar joint inertia. In practice, you obtain \(A,B\) either from analytic linearization or from robotics libraries (e.g., roboticstoolbox, Pinocchio, RBDL) that can linearize manipulator models.

We now define a finite-horizon quadratic cost for tracking a reference trajectory \(r_k\) (for example desired joint states or task-space trajectories mapped to joint space). Over a horizon length \(N\), with state and input weights \(Q \succeq 0\), \(R \succ 0\), and terminal weight \(P \succeq 0\), we define

\[ \begin{aligned} J_N(x_k,\mathbf{u}_k) &= x_{k+N\mid k}^{\top} P x_{k+N\mid k} + \sum_{i=0}^{N-1} \Big[ (x_{k+i\mid k} - r_{k+i})^{\top} Q (x_{k+i\mid k} - r_{k+i}) + u_{k+i\mid k}^{\top} R u_{k+i\mid k} \Big] \\ \mathbf{u}_k &= \begin{bmatrix} u_{k\mid k}^{\top} & u_{k+1\mid k}^{\top} & \dots & u_{k+N-1\mid k}^{\top} \end{bmatrix}^{\top} \end{aligned} \]

The predictions \(x_{k+i\mid k}\) are defined using the model \(x_{k+1} = A x_k + B u_k\) with initial state \(x_{k\mid k}=x_k\):

\[ \begin{aligned} x_{k+1\mid k} &= A x_{k\mid k} + B u_{k\mid k} \\ x_{k+2\mid k} &= A x_{k+1\mid k} + B u_{k+1\mid k} = A^2 x_{k\mid k} + A B u_{k\mid k} + B u_{k+1\mid k} \\ &\vdots \\ x_{k+i\mid k} &= A^i x_{k\mid k} + \sum_{j=0}^{i-1} A^{i-1-j} B u_{k+j\mid k},\quad i=1,\dots,N \end{aligned} \]

Proof by induction. For \(i=1\), the formula holds by definition. Assume it holds for some \(i\). Then

\[ \begin{aligned} x_{k+i+1\mid k} &= A x_{k+i\mid k} + B u_{k+i\mid k} \\ &= A\left( A^i x_{k\mid k} + \sum_{j=0}^{i-1} A^{i-1-j} B u_{k+j\mid k} \right) + B u_{k+i\mid k} \\ &= A^{i+1} x_{k\mid k} + \sum_{j=0}^{i-1} A^{i-j} B u_{k+j\mid k} + B u_{k+i\mid k} \\ &= A^{i+1} x_{k\mid k} + \sum_{j=0}^{i} A^{i-j} B u_{k+j\mid k} \end{aligned} \]

which is the claimed formula with \(i\) replaced by \(i+1\).

It is convenient to stack all predicted states and inputs:

\[ \mathbf{X}_k = \begin{bmatrix} x_{k+1\mid k} \\ \vdots \\ x_{k+N\mid k} \end{bmatrix} \in \mathbb{R}^{Nn},\quad \mathbf{U}_k = \begin{bmatrix} u_{k\mid k} \\ \vdots \\ u_{k+N-1\mid k} \end{bmatrix} \in \mathbb{R}^{Nm} \]

From the induction formula we can write

\[ \mathbf{X}_k = \mathcal{A} x_{k\mid k} + \mathcal{B} \mathbf{U}_k, \]

where

\[ \mathcal{A} = \begin{bmatrix} A \\ A^2 \\ \vdots \\ A^N \end{bmatrix},\quad \mathcal{B} = \begin{bmatrix} B & 0 & \cdots & 0 \\ A B & B & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ A^{N-1} B & A^{N-2} B & \cdots & B \end{bmatrix}. \]

Define block-diagonal weight matrices and stacked reference:

\[ \begin{aligned} Q_{\text{bar}} &= \operatorname{diag}(\underbrace{Q,\dots,Q}_{N-1\ \text{times}}, P), \quad R_{\text{bar}} = \operatorname{diag}(\underbrace{R,\dots,R}_{N\ \text{times}}), \\ \mathbf{R}_k &= \begin{bmatrix} r_{k} \\ \vdots \\ r_{k+N-1} \\ r_{k+N} \end{bmatrix}. \end{aligned} \]

Then the cost can be written in compact quadratic form:

\[ \begin{aligned} J_N(x_k,\mathbf{U}_k) &= (\mathcal{A}x_k + \mathcal{B}\mathbf{U}_k - \mathbf{R}_k)^{\top} Q_{\text{bar}} (\mathcal{A}x_k + \mathcal{B}\mathbf{U}_k - \mathbf{R}_k) + \mathbf{U}_k^{\top} R_{\text{bar}} \mathbf{U}_k \\ &= \mathbf{U}_k^{\top} H \mathbf{U}_k + 2 x_k^{\top} F^{\top} \mathbf{U}_k + \text{const}(x_k,\mathbf{R}_k), \end{aligned} \]

with

\[ H = \mathcal{B}^{\top} Q_{\text{bar}} \mathcal{B} + R_{\text{bar}},\quad F = \mathcal{B}^{\top} Q_{\text{bar}} \mathcal{A}. \]

If \(Q_{\text{bar}} \succeq 0\) and \(R_{\text{bar}} \succ 0\), then \(H \succ 0\) and the unconstrained minimizer is

\[ \mathbf{U}_k^{\star} = - H^{-1} F x_k. \]

Proof sketch. Take gradient of \(J_N\) with respect to \(\mathbf{U}_k\):

\[ \nabla_{\mathbf{U}_k} J_N = 2 H \mathbf{U}_k + 2 F x_k. \]

Setting this gradient to zero gives \(H \mathbf{U}_k^{\star} + F x_k = 0\). Since \(H\) is positive definite, it is invertible, so \(\mathbf{U}_k^{\star} = - H^{-1} F x_k\).

In a pure finite-horizon optimal control problem, we would compute \(\mathbf{U}_0^{\star}\) once and apply the entire sequence. The key idea of MPC is to instead:

1. Optimize over a horizon \(N\) from the current state.
2. Apply only the first control input \(u_k = u_{k\mid k}^{\star}\).
3. At the next time step, measure the new state and solve a new optimization problem.

This yields a feedback law, robust to disturbances and modeling errors, because the optimization is always anchored at the current state. The overall receding-horizon loop for a robot joint can be summarized as:

flowchart TD
  S["Start at time k"] --> X["Measure/estimate state x(k)"]
  X --> R["Build reference sequence r(k)...r(k+N)"]
  R --> PRED["Form prediction matrices A_bar, B_bar"]
  PRED --> OPT["Solve finite-horizon optimization for U_star"]
  OPT --> APPLY["Apply first input u(k) from U_star"]
  APPLY --> ADV["Advance to k = k+1"]
  ADV --> X
        

When constraints (on torque, velocity, etc.) are added in later lessons, the optimization in the node "Solve finite-horizon optimization" will be a quadratic program (QP) or, for nonlinear models, a nonlinear program (NLP).

From the condensed form, the optimal input sequence depends linearly on the current state \(x_k\) in the unconstrained case:

\[ \mathbf{U}_k^{\star} = - H^{-1} F x_k. \]

Define a selection matrix \(S\) that extracts the first control move:

\[ S = \begin{bmatrix} I_m & 0 & \dots & 0 \end{bmatrix}, \]

such that \(u_k = S \mathbf{U}_k\). The applied input is

\[ u_k = S \mathbf{U}_k^{\star} = - S H^{-1} F x_k = K_N x_k, \]

with feedback gain

\[ K_N = - S H^{-1} F. \]

Key point: even though MPC is formulated as an open-loop optimization problem at each step, the receding-horizon application of the first input defines a state-feedback law \(u_k = K_N x_k\) (in the unconstrained LTI, quadratic case).

Recall from Chapter 9 (Optimal Control) that the infinite-horizon LQR problem yields a stationary feedback gain \(K_{\infty}\) obtained from the discrete-time algebraic Riccati equation (DARE):

\[ \begin{aligned} P &= A^{\top} P A - A^{\top} P B (R + B^{\top} P B)^{-1} B^{\top} P A + Q, \\ K_{\infty} &= - (R + B^{\top} P B)^{-1} B^{\top} P A. \end{aligned} \]

If we choose the terminal weight \(P\) in the MPC cost to be the DARE solution above and let the horizon \(N\) be large enough, the receding-horizon law \(K_N\) coincides with \(K_{\infty}\) (for time-invariant systems with no constraints). This can be seen via dynamic programming:

• Finite-horizon optimal control with terminal cost \(P\) yields a sequence of Riccati matrices \(P_i\) and gains \(K_i\), \(i=N-1,\dots,0\).
• If \(P_N = P\) is the DARE solution, then the backward recursion leaves \(P_i=P\) and \(K_i=K_{\infty}\) invariant.
• The first step of the horizon always uses \(K_0 = K_{\infty}\), hence the receding horizon controller applies the LQR gain at each step.

Let \(V_N(x)\) denote the optimal value function (cost) of the MPC problem:

\[ V_N(x) = \min_{\mathbf{U}} J_N(x,\mathbf{U}). \]

Suppose:

• \((A,B)\) is stabilizable.
• \(Q \succeq 0\), \(R \succ 0\).
• The terminal weight \(P\) is chosen as the solution of the DARE for the corresponding infinite-horizon LQR.

Let \(u_k = \kappa_N(x_k)\) be the MPC law defined by the first element of \(\mathbf{U}_k^{\star}\). Then a classical result is that

\[ V_N(x_{k+1}) - V_N(x_k) \leq - x_k^{\top} Q x_k - u_k^{\top} R u_k, \]

which shows that \(V_N\) is a Lyapunov function for the closed-loop system. The idea of the proof is:

1. At time \(k\), let \(\mathbf{U}_k^{\star}\) be the optimal sequence for state \(x_k\).
2. At time \(k+1\), consider the candidate sequence obtained by shifting \(\mathbf{U}_k^{\star}\) by one step (dropping the first input and appending the LQR feedback at the end).
3. This candidate sequence is feasible and its cost upper-bounds \(V_N(x_{k+1})\).
4. Comparing the cost at \(x_k\) and at \(x_{k+1}\) yields the inequality above.

The inequality implies that the closed-loop system is asymptotically stable at the origin (for the unconstrained linear case). For robots, this provides a theoretical basis that the receding-horizon controller is not just a numerical trick but a Lyapunov-stable state-feedback law when designed appropriately.

In a practical robot control system, MPC is embedded among several other components: reference generators, state estimators, low-level torque/current control, and safety monitors. At the level of this course, we can abstract a typical architecture as:

flowchart TD
  REF["Reference trajectory r(k)"] --> MPC["MPC optimizer (receding horizon)"]
  EST["State estimator x_hat(k)"] --> MPC
  MPC --> TAU["Joint torque/velocity commands u(k)"]
  TAU --> ROBOT["Robot joints / actuators"]
  ROBOT --> SENS["Sensors (encoders, IMU, force)"]
  SENS --> EST
        

In later chapters (state estimation, digital implementation, safety), you will refine the estimator block and low-level actuation. Here, the focus is on the MPC optimizer: given \(x_k\), \(r_k\), and the model \(x_{k+1} = A x_k + B u_k\), compute \(u_k\) via a receding-horizon optimization.

Typical design choices for robots include:

• Sampling time \(T_s\): often between \(0.5\) ms and \(5\) ms for joint control.
• Horizon length \(N\): chosen so that \(N T_s\) spans several dominant mechanical time constants (e.g., \(0.1\)–\(0.5\) s).
• Weights \(Q, R, P\): tuned to trade off tracking accuracy and control effort, often based on LQR design plus heuristic adjustments.

We illustrate the receding-horizon principle on a simple 1-DOF joint modeled as a discrete-time double integrator (as in Section 2) with no constraints. We precompute the condensed matrices \(H\) and \(F\) and at each step apply \(u_k = - S H^{-1} F x_k\). For multi-DOF robots, matrices \(A,B\) would be obtained from rigid-body dynamics libraries:

• Python: roboticstoolbox, Pinocchio.
• C++: Eigen with RBDL or Pinocchio.
• Java: EJML or Apache Commons Math for matrices.
• MATLAB/Simulink: Robotics System Toolbox, Model Predictive Control Toolbox.
• Wolfram Mathematica: built-in linear systems and optimization functions.

8.1 Python Implementation (NumPy + Simple Simulation)

import numpy as np

# 1-DOF joint: state x = [q, qdot]^T, input u = torque
Ts = 0.02  # 20 ms sampling
I = 1.0    # inertia

A = np.array([[1.0, Ts],
              [0.0, 1.0]])
B = np.array([[0.5 * Ts**2 / I],
              [Ts / I]])

Q = np.diag([10.0, 1.0])
R = np.array([[0.1]])
P = Q.copy()   # simple choice; could use DARE solution for P

N = 20  # prediction horizon

n = A.shape[0]
m = B.shape[1]

# Build prediction matrices A_bar, B_bar
A_bar = np.zeros((N * n, n))
B_bar = np.zeros((N * n, N * m))

for i in range(N):
    A_power = np.linalg.matrix_power(A, i + 1)
    A_bar[i*n:(i+1)*n, :] = A_power
    for j in range(i + 1):
        A_ij = np.linalg.matrix_power(A, i - j)
        B_bar[i*n:(i+1)*n, j*m:(j+1)*m] = A_ij @ B

# Block-diagonal weights (no explicit reference tracking here, r_k = 0)
Q_bar = np.kron(np.eye(N - 1), Q)
Q_bar = np.block([
    [Q_bar, np.zeros(((N - 1) * n, n))],
    [np.zeros((n, (N - 1) * n)), P]
])
R_bar = np.kron(np.eye(N), R)

H = B_bar.T @ Q_bar @ B_bar + R_bar
F = B_bar.T @ Q_bar @ A_bar

# Pre-factorize H for speed (Cholesky or LDL can be used instead)
H_inv = np.linalg.inv(H)

def mpc_control(x):
    """
    One-step MPC: given current state x (2x1), compute u_k.
    Unconstrained linear-quadratic MPC; r_k = 0.
    """
    # Compute optimal sequence U_k^* = -H^{-1} F x_k
    U_star = - H_inv @ (F @ x)
    # Extract first control move
    u_k = U_star[0]
    return float(u_k)

# Simulate closed-loop response
T_final = 1.0
N_steps = int(T_final / Ts)

x = np.array([[0.5],   # initial position error (rad)
              [0.0]])  # initial velocity
trajectory = []
for k in range(N_steps):
    u = mpc_control(x)
    trajectory.append((k * Ts, float(x[0]), float(x[1]), u))
    # Plant update (discrete-time model)
    x = A @ x + B * u

# trajectory now contains (time, q, qdot, u) samples
      

In a robotic application, this Python code could be integrated with ROS 2 nodes and roboticstoolbox to obtain \(A,B\) from a manipulator model at each linearization point.

8.2 C++ Implementation (Eigen + Skeleton MPC Loop)

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

// For multi-DOF robots, obtain A,B from RBDL or Pinocchio.

using namespace Eigen;

int main() {
    const double Ts = 0.02;
    const double I  = 1.0;

    Matrix2d A;
    A << 1.0, Ts,
           0.0, 1.0;
    Vector2d B;
    B << 0.5 * Ts * Ts / I,
           Ts / I;

    Matrix2d Q;
    Q << 10.0, 0.0,
           0.0,  1.0;
    MatrixXd R(1,1);
    R(0,0) = 0.1;
    Matrix2d P = Q; // simple choice

    const int N = 20;
    const int n = 2;
    const int m = 1;

    MatrixXd A_bar = MatrixXd::Zero(N * n, n);
    MatrixXd B_bar = MatrixXd::Zero(N * n, N * m);

    for (int i = 0; i < N; ++i) {
        Matrix2d A_power = Matrix2d::Identity();
        for (int k = 0; k < i + 1; ++k) {
            A_power = A_power * A;
        }
        A_bar.block(i*n, 0, n, n) = A_power;

        for (int j = 0; j <= i; ++j) {
            Matrix2d A_ij = Matrix2d::Identity();
            for (int k = 0; k < i - j; ++k) {
                A_ij = A_ij * A;
            }
            B_bar.block(i*n, j*m, n, m) = A_ij * B;
        }
    }

    // Build Q_bar and R_bar
    MatrixXd Q_bar = MatrixXd::Zero(N * n, N * n);
    for (int i = 0; i < N - 1; ++i) {
        Q_bar.block(i*n, i*n, n, n) = Q;
    }
    Q_bar.block((N-1)*n, (N-1)*n, n, n) = P;

    MatrixXd R_bar = MatrixXd::Zero(N * m, N * m);
    for (int i = 0; i < N; ++i) {
        R_bar.block(i*m, i*m, m, m) = R;
    }

    MatrixXd H = B_bar.transpose() * Q_bar * B_bar + R_bar;
    MatrixXd F = B_bar.transpose() * Q_bar * A_bar;

    // Precompute LDLT factorization of H
    Eigen::LDLT<MatrixXd> ldlt(H);

    auto mpc_control = [&](const Vector2d& x) {
        VectorXd rhs = F * x;
        VectorXd U_star = - ldlt.solve(rhs);
        double u_k = U_star(0);
        return u_k;
    };

    double T_final = 1.0;
    int N_steps = static_cast<int>(T_final / Ts);

    Vector2d x;
    x << 0.5, 0.0;

    for (int k = 0; k < N_steps; ++k) {
        double u = mpc_control(x);
        x = A * x + B * u;
        std::cout << "k=" << k
                  << " q=" << x(0)
                  << " qdot=" << x(1)
                  << " u=" << u << std::endl;
    }

    return 0;
}
      

8.3 Java Implementation (EJML)

import org.ejml.simple.SimpleMatrix;

public class SimpleMPC {
    private static final double Ts = 0.02;
    private static final double I  = 1.0;
    private static final int N = 20;
    private static final int n = 2;
    private static final int m = 1;

    private final SimpleMatrix A;
    private final SimpleMatrix B;
    private final SimpleMatrix H;
    private final SimpleMatrix F;

    public SimpleMPC() {
        A = new SimpleMatrix(new double[][] {
            {1.0, Ts},
            {0.0, 1.0}
        });
        B = new SimpleMatrix(new double[][] {
            {0.5 * Ts * Ts / I},
            {Ts / I}
        });

        SimpleMatrix Q = SimpleMatrix.diag(10.0, 1.0);
        SimpleMatrix R = new SimpleMatrix(1,1);
        R.set(0,0, 0.1);
        SimpleMatrix P = Q.copy();

        SimpleMatrix A_bar = new SimpleMatrix(N * n, n);
        SimpleMatrix B_bar = new SimpleMatrix(N * n, N * m);

        for (int i = 0; i < N; ++i) {
            SimpleMatrix A_power = SimpleMatrix.identity(n);
            for (int k = 0; k < i + 1; ++k) {
                A_power = A_power.mult(A);
            }
            A_bar.insertIntoThis(i*n, 0, A_power);

            for (int j = 0; j <= i; ++j) {
                SimpleMatrix A_ij = SimpleMatrix.identity(n);
                for (int k = 0; k < i - j; ++k) {
                    A_ij = A_ij.mult(A);
                }
                SimpleMatrix block = A_ij.mult(B);
                B_bar.insertIntoThis(i*n, j*m, block);
            }
        }

        SimpleMatrix Q_bar = new SimpleMatrix(N * n, N * n);
        Q_bar.zero();
        for (int i = 0; i < N - 1; ++i) {
            Q_bar.insertIntoThis(i*n, i*n, Q);
        }
        Q_bar.insertIntoThis((N-1)*n, (N-1)*n, P);

        SimpleMatrix R_bar = new SimpleMatrix(N * m, N * m);
        R_bar.zero();
        for (int i = 0; i < N; ++i) {
            R_bar.insertIntoThis(i*m, i*m, R);
        }

        H = B_bar.transpose().mult(Q_bar).mult(B_bar).plus(R_bar);
        F = B_bar.transpose().mult(Q_bar).mult(A_bar);
    }

    public double mpcControl(SimpleMatrix x) {
        SimpleMatrix rhs = F.mult(x);
        SimpleMatrix U_star = H.solve(rhs).scale(-1.0);
        return U_star.get(0);
    }

    public static void main(String[] args) {
        SimpleMPC mpc = new SimpleMPC();
        SimpleMatrix x = new SimpleMatrix(2,1);
        x.set(0, 0, 0.5);
        x.set(1, 0, 0.0);

        int N_steps = (int)(1.0 / Ts);
        for (int k = 0; k < N_steps; ++k) {
            double u = mpc.mpcControl(x);
            x = mpc.A.mult(x).plus(mpc.B.scale(u));
            System.out.println("k=" + k +
                               " q=" + x.get(0) +
                               " qdot=" + x.get(1) +
                               " u=" + u);
        }
    }
}
      

8.4 MATLAB/Simulink Implementation

Ts = 0.02;
I  = 1.0;

A = [1 Ts;
     0 1];
B = [0.5*Ts^2/I;
     Ts/I];

Q = diag([10 1]);
R = 0.1;
P = Q;   % could be DARE solution

N = 20;
n = size(A,1);
m = size(B,2);

% Build prediction matrices
A_bar = zeros(N*n, n);
B_bar = zeros(N*n, N*m);

for i = 1:N
    A_power = A^i;
    A_bar((i-1)*n+1:i*n, :) = A_power;
    for j = 1:i
        A_ij = A^(i-j);
        B_bar((i-1)*n+1:i*n, (j-1)*m+1:j*m) = A_ij * B;
    end
end

Q_bar = blkdiag(kron(eye(N-1), Q), P);
R_bar = kron(eye(N), R);

H = B_bar' * Q_bar * B_bar + R_bar;
F = B_bar' * Q_bar * A_bar;

H_inv = inv(H);

mpc_control = @(x) - (H_inv * (F * x))(1);

% Closed-loop simulation
T_final = 1.0;
N_steps = round(T_final / Ts);

x = [0.5; 0.0];
for k = 1:N_steps
    u = mpc_control(x);
    x = A * x + B * u;
    fprintf('k=%d, q=%f, qdot=%f, u=%f\n', k-1, x(1), x(2), u);
end

% In Simulink, you can alternatively use:
%  - "State-Space" block for A,B,C,D
%  - "MPC Controller" block (Model Predictive Control Toolbox)
%  - Connect measured states (or estimates), reference, and constraints.
      

8.5 Wolfram Mathematica Implementation

Ts = 0.02;
I  = 1.0;

A = { {1.0, Ts},
     {0.0, 1.0} };
B = { {0.5 Ts^2/I},
     {Ts/I} };

Q = DiagonalMatrix[{10.0, 1.0}];
R = {{0.1}};
P = Q;  (* simple choice *)

N  = 20;
n  = Length[A];
m  = Length[B[[1]]];

(* Build Abar and Bbar *)
Abar = ConstantArray[0.0, {N*n, n}];
Bbar = ConstantArray[0.0, {N*n, N*m}];

Do[
  APower = MatrixPower[A, i];
  Abar[[ (i - 1)*n + 1 ;; i*n, 1 ;; n ]] = APower;
  Do[
    Aij = MatrixPower[A, i - j];
    Bblock = Aij.B;
    Bbar[[ (i - 1)*n + 1 ;; i*n, (j - 1)*m + 1 ;; j*m ]] = Bblock;
    ,
    {j, 1, i}
  ];
  ,
  {i, 1, N}
];

Qbar = ArrayFlatten[{
  Table[If[i == j && i < N, Q, If[i == j && i == N, P, ConstantArray[0.0, {n, n}]]],
        {i, 1, N}, {j, 1, N}]
}];

Rbar = ArrayFlatten[{
  Table[If[i == j, R, ConstantArray[0.0, {m, m}]],
        {i, 1, N}, {j, 1, N}]
}];

H = Transpose[Bbar].Qbar.Bbar + Rbar;
F = Transpose[Bbar].Qbar.Abar;

Hinv = Inverse[H];

mpcControl[x_] := Module[
  {Ustar},
  Ustar = - Hinv.(F.x);
  Ustar[[1, 1]]
];

(* Closed-loop simulation *)
Tfinal = 1.0;
Nsteps = Round[Tfinal/Ts];

x = { {0.5}, {0.0} };
traj = Table[
   u = mpcControl[x];
   x = A.x + B*u;
   {k*Ts, x[[1,1]], x[[2,1]], u}
   ,
   {k, 0, Nsteps - 1}
];

traj // TableForm
      

Problem 1 (Prediction Matrices for a 1D Integrator). Consider the scalar system \(x_{k+1} = a x_k + b u_k\) and a horizon \(N\). Derive the explicit matrices \(\mathcal{A}\) and \(\mathcal{B}\) such that \(\mathbf{X}_k = \mathcal{A} x_k + \mathcal{B} \mathbf{U}_k\).

Solution. The predicted states are

\[ \begin{aligned} x_{k+1\mid k} &= a x_k + b u_{k\mid k}, \\ x_{k+2\mid k} &= a^2 x_k + a b u_{k\mid k} + b u_{k+1\mid k}, \\ x_{k+3\mid k} &= a^3 x_k + a^2 b u_{k\mid k} + a b u_{k+1\mid k} + b u_{k+2\mid k}, \\ &\vdots \end{aligned} \]

Stacking \(\mathbf{X}_k = \begin{bmatrix} x_{k+1\mid k} & \dots & x_{k+N\mid k} \end{bmatrix}^{\top}\), we obtain

\[ \mathcal{A} = \begin{bmatrix} a \\ a^2 \\ \vdots \\ a^N \end{bmatrix},\quad \mathcal{B} = \begin{bmatrix} b & 0 & \dots & 0 \\ a b & b & \dots & 0 \\ a^2 b & a b & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ a^{N-1} b & a^{N-2} b & \dots & b \end{bmatrix}. \]

Problem 2 (Unconstrained MPC as State Feedback). For the linear-quadratic MPC problem in Section 3 with no constraints and reference \(r_k = 0\), show that the receding-horizon control law \(u_k = \kappa_N(x_k)\) is linear in \(x_k\).

Solution. From Section 3, the optimal sequence is

\[ \mathbf{U}_k^{\star} = - H^{-1} F x_k. \]

Let \(S\) be the selection matrix that picks the first control:

\[ u_k = S \mathbf{U}_k^{\star} = - S H^{-1} F x_k = K_N x_k. \]

Thus the receding-horizon MPC law is linear state feedback with gain \(K_N = - S H^{-1} F\).

Problem 3 (Horizon \(N=1\) and Static Feedback). Consider horizon \(N=1\) with cost \(J_1(x_k,u_k) = x_{k+1}^{\top} P x_{k+1} + x_k^{\top} Q x_k + u_k^{\top} R u_k\) for the system \(x_{k+1} = A x_k + B u_k\). Show that the optimal control law is a static state feedback \(u_k = K_1 x_k\) and derive \(K_1\).

Solution. We have

\[ \begin{aligned} J_1(x_k,u_k) &= (A x_k + B u_k)^{\top} P (A x_k + B u_k) + x_k^{\top} Q x_k + u_k^{\top} R u_k \\ &= x_k^{\top}(A^{\top} P A + Q) x_k + 2 x_k^{\top} A^{\top} P B u_k + u_k^{\top}(B^{\top} P B + R) u_k. \end{aligned} \]

This is a quadratic function in \(u_k\) with Hessian \(B^{\top} P B + R \succ 0\). Differentiating and setting to zero:

\[ 2 (B^{\top} P B + R) u_k + 2 B^{\top} P A x_k = 0, \]

so

\[ u_k = - (B^{\top} P B + R)^{-1} B^{\top} P A x_k = K_1 x_k. \]

Problem 4 (Lyapunov Decrease for Unconstrained MPC). Under the assumptions of Section 6, outline why \(V_N(x_{k+1}) - V_N(x_k) \leq - \ell(x_k,u_k)\) with \(\ell(x,u) = x^{\top} Q x + u^{\top} R u\).

Solution. Let \(\mathbf{U}_k^{\star}\) be optimal at time \(k\) for state \(x_k\). Apply the first control \(u_k\) to obtain \(x_{k+1}\). At time \(k+1\), consider the shifted input sequence

\[ \tilde{\mathbf{U}}_{k+1} = \begin{bmatrix} u_{k+1\mid k}^{\star} \\ \vdots \\ u_{k+N-1\mid k}^{\star} \\ K_{\infty} x_{k+N\mid k}^{\star} \end{bmatrix}, \]

where \(K_{\infty}\) is the LQR gain corresponding to terminal weight \(P\). This sequence is feasible for the MPC problem at \(x_{k+1}\). Its cost is equal to \(J_N(x_k,\mathbf{U}_k^{\star}) - \ell(x_k,u_k)\) by cancellation of overlapping terms and use of the DARE identity. Since the optimal cost at \(x_{k+1}\) is no larger than the cost of this candidate sequence, we obtain

\[ V_N(x_{k+1}) \leq V_N(x_k) - \ell(x_k,u_k), \]

which is the desired Lyapunov decrease inequality.

Problem 5 (Receding Horizon vs. Open-Loop Execution). Explain why, in the presence of unmodeled disturbances or modeling errors, applying the entire planned sequence \(\mathbf{U}_0^{\star}\) open-loop over the whole horizon is generally inferior to applying only the first element at each step and re-optimizing (receding horizon).

Solution. In open-loop execution, the state trajectory evolves according to the nominal predictions used in planning. Any disturbance or modeling mismatch will cause the actual state to drift away from the predicted one, but the controller does not react because the remaining inputs are fixed. In contrast, receding-horizon MPC re-plans at each step based on the measured (or estimated) state. The optimizer then computes a new optimal sequence under the updated conditions, compensating for accumulated errors. Thus receding horizon introduces feedback through repeated optimization, increasing robustness, while open-loop execution of the full sequence does not.

In this lesson we introduced the receding-horizon principle at the core of MPC. Starting from linearized robot joint dynamics, we built a discrete-time prediction model and derived the stacked (condensed) representation of predicted states and inputs. We showed how finite-horizon quadratic costs lead to a quadratic program, and in the unconstrained case, to an explicit linear state-feedback law \(u_k = K_N x_k\).

We connected MPC with infinite-horizon LQR via the Riccati equation and outlined how appropriate choice of terminal weight \(P\) makes the MPC law coincide with the LQR feedback. Finally, we sketched a Lyapunov-based stability argument and implemented the receding-horizon controller for a 1-DOF joint in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.

In the next lesson, we will formulate MPC specifically for robot tracking problems, incorporating reference trajectories more explicitly and preparing the ground for handling hard constraints on torques, velocities, and joint limits.