After this lesson, the student should be able to:

• separate regulatory, supervisory, and economic objectives by time scale;
• formulate a steady-state economic target problem and a finite-horizon tracking MPC problem;
• define the signals exchanged between an adaptive lower layer and an MPC/optimization upper layer;
• allocate actuator authority so that adaptive corrections cannot invalidate supervisory constraints;
• explain why online parameter updates can destroy nominal MPC recursive feasibility;
• construct a certified model-update gate, reference governor, and fallback policy;
• derive practical stability conditions for the interconnected hierarchy.

This is an adaptive-control lesson, not a complete MPC course. We use the standard constrained finite-horizon formulation only to analyze its interaction with adaptation. Detailed QP algorithms, terminal-set construction, and nonlinear MPC are treated in dedicated MPC courses.

Process and power systems usually contain objectives with incompatible bandwidths. A fast layer must reject disturbances and stabilize local dynamics. A supervisory layer coordinates multivariable constraints and anticipates future behavior. A slow optimization layer changes the economically preferred operating point as prices, loads, production demands, or equipment availability change.

A useful three-time-scale ordering is \( T_{\mathrm{local}} \ll T_{\mathrm{MPC}} \ll T_{\mathrm{econ}} \). The ordering does not by itself prove stability, but it makes the interconnection analyzable: each upper layer appears approximately constant to the faster layer, while each lower layer appears as a stabilized closed-loop subsystem to the slower layer.

flowchart TD
  D["Demand, price,
and production objective"] --> RTO["Economic optimizer
and RTO"]

  RTO -->|"Optimal target
and economic weights"| MPC["Supervisory MPC"]

  MPC -->|"Nominal trajectory,
feedforward input,
and constraint budget"| LOCAL["Fast adaptive
local controller"]

  LOCAL --> PLANT["Process or
power-stage dynamics"]

  PLANT -->|"Measurements"| LOCAL

  LOCAL -->|"Estimated parameters,
covariance, and
saturation flags"| MPC

  MPC -->|"Predicted constraints
and feasible target range"| RTO

  SAFE["Safety interlocks
and fallback logic"] --> MPC
  SAFE --> LOCAL
  

Consider a sampled uncertain plant

\[ x_{k+1}=A(\theta^\star)x_k+B(\theta^\star)u_k+Ew_k,\qquad y_k=Cx_k, \]

where \( \theta^\star \) is an unknown or slowly varying parameter vector. The local adaptive layer produces a correction around the nominal supervisory command:

\[ u_k=u^{\mathrm{nom}}_k+u^{\mathrm{ad}}_k,\qquad u^{\mathrm{ad}}_k=K_e(\hat\theta_k) \left(x^{\mathrm{nom}}_k-x_k\right)+u^{\mathrm{ff}}_{\mathrm{ad},k}. \]

The upper layer should not receive only a point estimate. A more useful interface packet is

\[ \mathcal{I}_k= \left\{\hat\theta_k,\;P_k,\;\Theta_k,\; s^{\mathrm{sat}}_k,\;q^{\mathrm{data}}_k,\; \bar u^{\mathrm{ad}}_k,\;\bar e_k\right\}, \]

where \( P_k \) is an estimator covariance or information measure, \( \Theta_k \) is an admissible parameter set, \( s^{\mathrm{sat}}_k \) reports saturation, \( q^{\mathrm{data}}_k \) reports data quality or excitation, and \( \bar u^{\mathrm{ad}}_k \) bounds the authority required by the local correction.

The slow layer commonly computes an economically attractive admissible steady state. For a certified parameter vector \( \hat\theta_c \) and exogenous economic data \( p \), define

\[ \begin{aligned} \min_{x_s,u_s}\quad & \ell_{\mathrm{econ}}(x_s,u_s;p)\\ \text{subject to}\quad & x_s=A(\hat\theta_c)x_s+B(\hat\theta_c)u_s,\\ & x_s\in\mathcal{X}_{s},\qquad u_s\in\mathcal{U}_{s},\\ & g_{\mathrm{process}}(x_s,u_s;p)\le 0. \end{aligned} \]

The sets \( \mathcal{X}_s \) and \( \mathcal{U}_s \) are normally stricter than the hard physical limits. The margin reserves room for transients, uncertainty, local adaptive correction, and unmeasured disturbances.

For the scalar implementation later in this lesson, the economic target solves

\[ \min_{r}\;q_d(r-r_d)^2+c_p p r^2, \qquad r_{\mathrm{econ}}= \frac{q_d}{q_d+c_p p}r_d. \]

Thus higher energy price \( p \) moves the requested operating point away from the production demand \( r_d \). This target is not sent directly to the fast controller; it first passes through feasibility and rate checks.

At supervisory instant \( K \), freeze the certified model over the prediction horizon and solve

\[ \begin{aligned} V_N(x_K,r_K,\hat\theta_c)= \min_{\mathbf{v},\boldsymbol{\epsilon}}\quad& \sum_{i=0}^{N-1} \left( \|x_{i|K}-x_s\|_Q^2+ \|v_{i|K}-u_s\|_R^2+ \|\Delta v_{i|K}\|_S^2+ \rho\|\epsilon_{i|K}\|^2 \right)\\ &+\|x_{N|K}-x_s\|_P^2\\ \text{subject to}\quad& x_{i+1|K}=A(\hat\theta_c)x_{i|K} +B(\hat\theta_c)v_{i|K},\\ &x_{i|K}\in\mathcal{X}_{\mathrm{tight}}\oplus\epsilon_{i|K},\\ &v_{i|K}\in\mathcal{U}_{\mathrm{sup}},\qquad \Delta v_{i|K}\in\Delta\mathcal{U},\\ &x_{N|K}\in\mathcal{X}_f. \end{aligned} \]

Only the first nominal move is implemented before the optimization is repeated. In a hierarchical implementation, the planned sequence also supplies a nominal state trajectory \( x^{\mathrm{nom}}_{i|K} \) around which the local adaptive loop corrects fast mismatch.

The supervisor should use a model that remains fixed during one solve. Allowing the estimator to alter the prediction model during QP iterations makes the optimization problem ill-defined and complicates real-time certification.

For a linearly parameterized one-step predictor

\[ x_{k+1}=\phi_k^\top\theta^\star+\nu_k, \]

projected RLS can be written as

\[ \begin{aligned} K_k&=\frac{P_k\phi_k} {\lambda+\phi_k^\top P_k\phi_k},\\ \hat\theta_{k+1}^{-} &=\hat\theta_k+ K_k\left(x_{k+1}-\phi_k^\top\hat\theta_k\right),\\ P_{k+1}&=\frac{1}{\lambda} \left(P_k-K_k\phi_k^\top P_k\right),\\ \hat\theta_{k+1} &=\operatorname{Proj}_{\Theta} \left(\hat\theta_{k+1}^{-}\right). \end{aligned} \]

The estimator output is a candidate model, not automatically a controller model. Define an acceptance map \( \mathcal{G} \):

\[ \hat\theta_{c,K+1}= \begin{cases} \hat\theta_{K+1},& \mathcal{G}(\hat\theta_{K+1},P_{K+1}, \mathcal{D}_{K+1})=1,\\ \hat\theta_{c,K},&\text{otherwise}. \end{cases} \]

Typical gate conditions include bounded parameters, known control direction, acceptable covariance, sufficiently small one-step validation error, no sensor-quality alarm, and successful feasibility testing with the new model. The previous certified model remains the fallback.

If the MPC plans with the entire actuator range and the adaptive layer later adds a correction, hard constraints can be violated even when the MPC solution was feasible. The correct allocation is

\[ \mathcal{U}_{\mathrm{sup}} \oplus\mathcal{U}_{\mathrm{ad}} \oplus\mathcal{U}_{\mathrm{dist}} \subseteq\mathcal{U}_{\mathrm{hard}}. \]

For a scalar symmetric actuator this becomes

\[ \bar u_{\mathrm{sup}}+ \bar u_{\mathrm{ad}}+ \bar u_{\mathrm{dist}} \le \bar u_{\mathrm{hard}}. \]

The local controller must also obey anti-windup or adaptation-freezing logic. A common rule is to halt the component of the parameter update that would demand still more saturated control:

\[ \dot{\hat\theta}= \begin{cases} \Gamma\phi e,&u\text{ is not saturated},\\ \Pi_{\mathrm{safe}}(\Gamma\phi e),& u\text{ is saturated}. \end{cases} \]

This does not guarantee parameter convergence, but it prevents the adaptive law from interpreting an unavailable actuator command as unexplained plant dynamics.

An economically optimal target can be unreachable from the current state under input, slew-rate, and state constraints. A scalar reference governor computes

\[ r_K=r_{K-1}+\alpha_K \left(r_{\mathrm{econ},K}-r_{K-1}\right), \qquad \alpha_K\in[0,1], \]

where \( \alpha_K \) is the largest value that preserves feasibility of a short prediction test. A simpler industrial approximation is

\[ |r_K-r_{K-1}|\le\Delta r_{\max}. \]

Bumpless transfer also requires consistent controller states. When a supervisory command changes, initialize the local integral or adaptive feedforward state so that the commanded actuator value is continuous:

\[ u^{\mathrm{local}}_{K^-} =u^{\mathrm{nom}}_{K^+} +u^{\mathrm{ad}}_{K^+}. \]

Standard MPC recursive-feasibility arguments shift the previously feasible input sequence and append a terminal control action. That argument assumes the prediction dynamics and constraint sets used at \( K+1 \) are compatible with those used at \( K \). An adaptive update changes the dynamics:

\[ A(\hat\theta_{c,K+1})\ne A(\hat\theta_{c,K}), \qquad B(\hat\theta_{c,K+1})\ne B(\hat\theta_{c,K}). \]

Therefore the shifted sequence need not satisfy the new predicted state constraints or terminal condition. Three standard remedies are:

1. robust/tube tightening for every model in a maintained set \( \Theta_K \);
2. a bounded-update rule \( \|\hat\theta_{c,K+1}-\hat\theta_{c,K}\|\le\delta_\theta \) plus continuity margins;
3. a feasibility gate that tests the new model and rejects it when the warm-start sequence becomes infeasible.

In safety-critical deployment, model acceptance and control optimization are distinct software transactions. A new estimate is not active until it passes validation and the controller atomically swaps to the certified version.

Let \( e_k=x_k-x_k^{\mathrm{nom}} \) denote the local tracking error. Assume the adaptive/local layer admits an ISS-like Lyapunov inequality

\[ V_a(k+1)-V_a(k) \le -\alpha_e\|e_k\|^2+ \gamma_r\|\Delta r_K\|^2+ \gamma_\theta\|\tilde\theta_k\|^2+ \gamma_w\|w_k\|^2. \]

Suppose the MPC value function satisfies, for a fixed certified model,

\[ V_N(K+1)-V_N(K) \le -\alpha_z\|z_K\|^2+ L_e\|e_K\|+ L_\theta\|\Delta\hat\theta_{c,K}\|, \]

where \( z_K \) is the nominal regulation error. Define the composite candidate

\[ W_K=V_N(K)+cV_a(K). \]

Applying Young's inequality \(L_e\|e\|\le \frac{\eta}{2}\|e\|^2+\frac{L_e^2}{2\eta}\) gives

\[ \begin{aligned} W_{K+1}-W_K \le& -\alpha_z\|z_K\|^2 -\left(c\alpha_e-\frac{\eta}{2}\right)\|e_K\|^2\\ &+c\gamma_r\|\Delta r_K\|^2 +c\gamma_\theta\|\tilde\theta_K\|^2 +L_\theta\|\Delta\hat\theta_{c,K}\| +c\gamma_w\|w_K\|^2 +\frac{L_e^2}{2\eta}. \end{aligned} \]

Choose \( c\alpha_e>\eta/2 \). If target moves, model swaps, estimation errors, and disturbances are bounded and sufficiently slow/small, then \( W_K \) decreases outside a residual set. The conclusion is practical stability rather than exact asymptotic convergence. Exact convergence additionally requires vanishing reference motion, disturbance, and model error.

sequenceDiagram
  participant S as Sensors
  participant E as Estimator
  participant G as Model
gate
  participant M as Supervisory
MPC
  participant L as Local adaptive
loop
  participant P as Plant

  S->>E: Measurement
packet

  E->>G: Candidate model,
covariance, and quality flags

  G->>G: Parameter bounds,
validation, and feasibility check

  alt Candidate accepted
    G->>M: Activate certified
model version
  else Candidate rejected
    G->>M: Retain previous
certified model
  end

  M->>M: Solve constrained
horizon problem

  M->>L: Nominal state and input
with authority budget

  loop Fast control samples
    L->>P: Corrected and
constrained input
    P->>S: Measured state
and output
  end
  

Recommended implementation rules are:

1. timestamp every measurement, estimate, model, and plan;
2. freeze one certified model during each optimization solve;
3. version the model and reject stale optimization results;
4. monitor solver status, iteration limit, and constraint residuals;
5. retain a last-feasible plan and a verified local fallback controller;
6. separate hard interlocks from adaptive and optimization software.

The implementations use

\[ x_{k+1}=a^\star_k x_k+b^\star u_k+d^\star_k, \qquad \phi_k=\begin{bmatrix}x_k&u_k&1\end{bmatrix}^{\top}, \qquad \theta^\star_k= \begin{bmatrix}a^\star_k&b^\star&d^\star_k\end{bmatrix}^{\top}. \]

The true pole changes from \(0.92\) to \(0.98\) at sample 150. Projected RLS estimates \(a,b,d\). Every five samples, the economic layer computes a price-dependent target, a rate governor limits the target motion, and the supervisory optimizer computes a ten-step nominal input/state plan. At every sample, a fast feedback correction is added around the planned state.

The teaching optimizer uses projected finite-difference gradient descent so that all five language versions are self-contained. In a production implementation, use a validated QP/NLP solver and verify worst-case execution time.

Libraries: NumPy is used for matrix operations; Matplotlib is optional. For production MPC, common choices include CVXPY with OSQP, CasADi, do-mpc, and acados.

Chapter27_Lesson4.py

# Chapter27_Lesson4.py
# Hierarchical adaptive control:
# economic target -> supervisory MPC -> fast local correction -> plant
#
# Dependencies:
#   required: numpy
#   optional: matplotlib (for plots)

from __future__ import annotations

import csv
from dataclasses import dataclass
from math import pi, sin
from pathlib import Path

import numpy as np


def clamp(value: float, lower: float, upper: float) -> float:
    return max(lower, min(upper, value))


def project_plan(
    plan: np.ndarray,
    previous_input: float,
    input_min: float,
    input_max: float,
    delta_input_max: float,
) -> np.ndarray:
    """Project a candidate sequence onto input and slew-rate constraints."""
    projected = np.empty_like(plan, dtype=float)
    last = previous_input
    for index, candidate in enumerate(plan):
        lower = max(input_min, last - delta_input_max)
        upper = min(input_max, last + delta_input_max)
        projected[index] = clamp(float(candidate), lower, upper)
        last = projected[index]
    return projected


def rollout_cost(
    plan: np.ndarray,
    x0: float,
    a_hat: float,
    b_hat: float,
    d_hat: float,
    reference: float,
    price: float,
    previous_input: float,
    state_tight_limit: float,
) -> float:
    """Finite-horizon tracking/economic cost with a soft state constraint."""
    q_tracking = 4.0
    r_energy = 0.08
    r_move = 0.50
    q_terminal = 8.0
    state_penalty = 200.0

    x = x0
    last_u = previous_input
    total = 0.0
    for u in plan:
        x = a_hat * x + b_hat * u + d_hat
        violation = max(0.0, abs(x) - state_tight_limit)
        total += (
            q_tracking * (x - reference) ** 2
            + r_energy * price * u**2
            + r_move * (u - last_u) ** 2
            + state_penalty * violation**2
        )
        last_u = float(u)

    total += q_terminal * (x - reference) ** 2
    return float(total)


def solve_supervisory_mpc(
    x0: float,
    model: np.ndarray,
    reference: float,
    price: float,
    previous_input: float,
    warm_start: np.ndarray | None,
    horizon: int = 10,
) -> tuple[np.ndarray, np.ndarray]:
    """
    Projected-gradient MPC for the scalar teaching example.

    Production implementations should normally use a QP/NLP solver such as
    OSQP, CVXPY, qpOASES, acados, or a vendor real-time solver.
    """
    a_hat, b_hat, d_hat = model
    input_min, input_max = -2.0, 2.0
    delta_input_max = 0.35
    state_tight_limit = 3.60

    if warm_start is None:
        plan = np.full(horizon, previous_input, dtype=float)
    else:
        plan = np.asarray(warm_start, dtype=float).copy()
        if plan.size < horizon:
            fill = plan[-1] if plan.size else previous_input
            plan = np.concatenate([plan, np.full(horizon - plan.size, fill)])
        plan = plan[:horizon]

    plan = project_plan(
        plan, previous_input, input_min, input_max, delta_input_max
    )

    step_size = 0.08
    finite_difference = 1.0e-4
    for _ in range(35):
        base_cost = rollout_cost(
            plan,
            x0,
            a_hat,
            b_hat,
            d_hat,
            reference,
            price,
            previous_input,
            state_tight_limit,
        )

        gradient = np.zeros(horizon)
        for j in range(horizon):
            perturbed = plan.copy()
            perturbed[j] += finite_difference
            perturbed = project_plan(
                perturbed,
                previous_input,
                input_min,
                input_max,
                delta_input_max,
            )
            gradient[j] = (
                rollout_cost(
                    perturbed,
                    x0,
                    a_hat,
                    b_hat,
                    d_hat,
                    reference,
                    price,
                    previous_input,
                    state_tight_limit,
                )
                - base_cost
            ) / finite_difference

        candidate = project_plan(
            plan - step_size * gradient,
            previous_input,
            input_min,
            input_max,
            delta_input_max,
        )
        candidate_cost = rollout_cost(
            candidate,
            x0,
            a_hat,
            b_hat,
            d_hat,
            reference,
            price,
            previous_input,
            state_tight_limit,
        )

        if candidate_cost < base_cost:
            plan = candidate
            step_size = min(0.12, 1.05 * step_size)
        else:
            step_size *= 0.5

        if np.linalg.norm(gradient) < 1.0e-4:
            break

    predicted_state = np.empty(horizon + 1)
    predicted_state[0] = x0
    for i, u in enumerate(plan):
        predicted_state[i + 1] = (
            a_hat * predicted_state[i] + b_hat * u + d_hat
        )

    return plan, predicted_state


@dataclass
class ProjectedRLS:
    theta: np.ndarray
    covariance: np.ndarray
    forgetting_factor: float = 0.998

    def update(self, x: float, u: float, x_next: float) -> None:
        phi = np.array([x, u, 1.0])
        denominator = (
            self.forgetting_factor + phi @ self.covariance @ phi
        )
        gain = self.covariance @ phi / denominator
        prediction_error = x_next - phi @ self.theta
        self.theta = self.theta + gain * prediction_error
        self.covariance = (
            self.covariance
            - np.outer(gain, phi) @ self.covariance
        ) / self.forgetting_factor

        # Projection is a safety mechanism, not a substitute for excitation.
        self.theta[0] = clamp(self.theta[0], 0.60, 1.08)
        self.theta[1] = clamp(self.theta[1], 0.08, 0.50)
        self.theta[2] = clamp(self.theta[2], -0.20, 0.20)


def model_is_certifiable(theta: np.ndarray) -> bool:
    """Simple acceptance gate before a new model reaches the MPC layer."""
    a_hat, b_hat, d_hat = theta
    return (
        0.60 <= a_hat <= 1.08
        and 0.08 <= b_hat <= 0.50
        and abs(d_hat) <= 0.20
    )


def run_simulation(steps: int = 300) -> list[dict[str, float]]:
    supervisory_period = 5
    horizon = 10
    local_feedback_gain = 1.8

    x = 0.0
    previous_input = 0.0
    governed_reference = 0.0

    rls = ProjectedRLS(
        theta=np.array([0.85, 0.22, 0.0], dtype=float),
        covariance=20.0 * np.eye(3),
    )
    certified_model = rls.theta.copy()

    planned_input = np.zeros(horizon)
    planned_state = np.zeros(horizon + 1)
    records: list[dict[str, float]] = []

    for k in range(steps):
        price = 1.0 + 0.35 * sin(2.0 * pi * k / 90.0)
        demand = 2.4 if k < 210 else 1.6

        if k % supervisory_period == 0:
            # Steady-state economic target:
            # min_r 5(r-demand)^2 + 0.7*price*r^2
            economic_reference = 5.0 * demand / (5.0 + 0.7 * price)

            # Reference governor: avoid abrupt target changes.
            governed_reference += clamp(
                economic_reference - governed_reference, -0.18, 0.18
            )

            # Certified model hand-off. If the candidate fails the gate,
            # the MPC continues with the last accepted model.
            if model_is_certifiable(rls.theta):
                certified_model = rls.theta.copy()

            warm_start = np.concatenate(
                [planned_input[1:], planned_input[-1:]]
            )
            planned_input, planned_state = solve_supervisory_mpc(
                x0=x,
                model=certified_model,
                reference=governed_reference,
                price=price,
                previous_input=previous_input,
                warm_start=warm_start,
                horizon=horizon,
            )

        plan_index = min(k % supervisory_period, horizon - 1)
        nominal_u = float(planned_input[plan_index])
        nominal_x = float(planned_state[plan_index])

        # Fast layer corrects inter-sample/model mismatch around the MPC plan.
        u = clamp(
            nominal_u + local_feedback_gain * (nominal_x - x),
            -2.0,
            2.0,
        )

        # Plant changes at k=150 to emulate operating-point variation.
        a_true = 0.92 if k < 150 else 0.98
        b_true = 0.22
        d_true = 0.03 + 0.01 * sin(2.0 * pi * k / 50.0)
        x_next = a_true * x + b_true * u + d_true

        rls.update(x, u, x_next)

        records.append(
            {
                "k": float(k),
                "x": x,
                "u": u,
                "reference": governed_reference,
                "price": price,
                "a_hat": float(rls.theta[0]),
                "b_hat": float(rls.theta[1]),
                "d_hat": float(rls.theta[2]),
                "a_cert": float(certified_model[0]),
                "b_cert": float(certified_model[1]),
                "d_cert": float(certified_model[2]),
            }
        )

        x = x_next
        previous_input = u

    return records


def save_csv(records: list[dict[str, float]], path: Path) -> None:
    with path.open("w", newline="", encoding="utf-8") as stream:
        writer = csv.DictWriter(stream, fieldnames=records[0].keys())
        writer.writeheader()
        writer.writerows(records)


def plot_results(records: list[dict[str, float]]) -> None:
    try:
        import matplotlib.pyplot as plt
    except ImportError:
        print("matplotlib is not installed; CSV output was still generated.")
        return

    k = np.array([row["k"] for row in records])
    x = np.array([row["x"] for row in records])
    reference = np.array([row["reference"] for row in records])
    u = np.array([row["u"] for row in records])
    a_hat = np.array([row["a_hat"] for row in records])

    plt.figure()
    plt.plot(k, x, label="state")
    plt.plot(k, reference, "--", label="governed reference")
    plt.xlabel("sample")
    plt.ylabel("state")
    plt.grid(True)
    plt.legend()
    plt.tight_layout()

    plt.figure()
    plt.plot(k, u)
    plt.xlabel("sample")
    plt.ylabel("input")
    plt.grid(True)
    plt.tight_layout()

    plt.figure()
    plt.plot(k, a_hat, label="estimated a")
    plt.axvline(150, linestyle="--", label="plant change")
    plt.xlabel("sample")
    plt.ylabel("parameter")
    plt.grid(True)
    plt.legend()
    plt.tight_layout()
    plt.show()


if __name__ == "__main__":
    output_path = Path("Chapter27_Lesson4_results.csv")
    simulation_records = run_simulation()
    save_csv(simulation_records, output_path)

    state = np.array([row["x"] for row in simulation_records])
    reference = np.array([row["reference"] for row in simulation_records])
    rmse = float(np.sqrt(np.mean((state - reference) ** 2)))

    print(f"Saved {output_path}")
    print(f"Tracking RMSE: {rmse:.6f}")
    print(f"Maximum absolute state: {np.max(np.abs(state)):.6f}")
    plot_results(simulation_records)

This version uses only the C++17 standard library. Larger systems normally use Eigen for linear algebra and OSQP-Eigen, qpOASES, CasADi, or acados for optimization.

Chapter27_Lesson4.cpp

// Chapter27_Lesson4.cpp
// Build: g++ -std=c++17 -O2 Chapter27_Lesson4.cpp -o Chapter27_Lesson4
// Run:   ./Chapter27_Lesson4

#include <algorithm>
#include <array>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <vector>

constexpr double PI = 3.14159265358979323846;

double clampValue(double value, double lower, double upper) {
    return std::max(lower, std::min(upper, value));
}

std::vector<double> projectPlan(
    const std::vector<double>& plan,
    double previousInput,
    double inputMin = -2.0,
    double inputMax = 2.0,
    double deltaInputMax = 0.35) {

    std::vector<double> projected(plan.size());
    double last = previousInput;
    for (std::size_t i = 0; i < plan.size(); ++i) {
        const double lower = std::max(inputMin, last - deltaInputMax);
        const double upper = std::min(inputMax, last + deltaInputMax);
        projected[i] = clampValue(plan[i], lower, upper);
        last = projected[i];
    }
    return projected;
}

double rolloutCost(
    const std::vector<double>& plan,
    double x0,
    const std::array<double, 3>& model,
    double reference,
    double price,
    double previousInput) {

    const double qTracking = 4.0;
    const double rEnergy = 0.08;
    const double rMove = 0.50;
    const double qTerminal = 8.0;
    const double statePenalty = 200.0;
    const double stateTightLimit = 3.60;

    double x = x0;
    double lastU = previousInput;
    double total = 0.0;

    for (double u : plan) {
        x = model[0] * x + model[1] * u + model[2];
        const double violation = std::max(0.0, std::abs(x) - stateTightLimit);
        total += qTracking * std::pow(x - reference, 2.0)
               + rEnergy * price * u * u
               + rMove * std::pow(u - lastU, 2.0)
               + statePenalty * violation * violation;
        lastU = u;
    }

    total += qTerminal * std::pow(x - reference, 2.0);
    return total;
}

struct MpcPlan {
    std::vector<double> input;
    std::vector<double> state;
};

MpcPlan solveSupervisoryMpc(
    double x0,
    const std::array<double, 3>& model,
    double reference,
    double price,
    double previousInput,
    const std::vector<double>& warmStart,
    int horizon = 10) {

    std::vector<double> plan(horizon, previousInput);
    for (int i = 0; i < std::min(horizon, static_cast<int>(warmStart.size())); ++i) {
        plan[i] = warmStart[i];
    }
    plan = projectPlan(plan, previousInput);

    double stepSize = 0.08;
    const double finiteDifference = 1.0e-4;

    for (int iteration = 0; iteration < 35; ++iteration) {
        const double baseCost = rolloutCost(
            plan, x0, model, reference, price, previousInput);

        std::vector<double> gradient(horizon, 0.0);
        double gradientNormSquared = 0.0;

        for (int j = 0; j < horizon; ++j) {
            std::vector<double> perturbed = plan;
            perturbed[j] += finiteDifference;
            perturbed = projectPlan(perturbed, previousInput);
            gradient[j] = (
                rolloutCost(
                    perturbed, x0, model, reference, price, previousInput)
                - baseCost
            ) / finiteDifference;
            gradientNormSquared += gradient[j] * gradient[j];
        }

        std::vector<double> candidate(horizon);
        for (int j = 0; j < horizon; ++j) {
            candidate[j] = plan[j] - stepSize * gradient[j];
        }
        candidate = projectPlan(candidate, previousInput);

        const double candidateCost = rolloutCost(
            candidate, x0, model, reference, price, previousInput);

        if (candidateCost < baseCost) {
            plan = candidate;
            stepSize = std::min(0.12, 1.05 * stepSize);
        } else {
            stepSize *= 0.5;
        }

        if (std::sqrt(gradientNormSquared) < 1.0e-4) {
            break;
        }
    }

    std::vector<double> predictedState(horizon + 1, 0.0);
    predictedState[0] = x0;
    for (int i = 0; i < horizon; ++i) {
        predictedState[i + 1] =
            model[0] * predictedState[i] + model[1] * plan[i] + model[2];
    }

    return {plan, predictedState};
}

class ProjectedRls {
public:
    ProjectedRls() : theta{0.85, 0.22, 0.0}, forgettingFactor(0.998) {
        for (auto& row : covariance) {
            row.fill(0.0);
        }
        for (int i = 0; i < 3; ++i) {
            covariance[i][i] = 20.0;
        }
    }

    void update(double x, double u, double xNext) {
        const std::array<double, 3> phi{x, u, 1.0};

        std::array<double, 3> pPhi{};
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                pPhi[i] += covariance[i][j] * phi[j];
            }
        }

        double denominator = forgettingFactor;
        for (int i = 0; i < 3; ++i) {
            denominator += phi[i] * pPhi[i];
        }

        std::array<double, 3> gain{};
        for (int i = 0; i < 3; ++i) {
            gain[i] = pPhi[i] / denominator;
        }

        double prediction = 0.0;
        for (int i = 0; i < 3; ++i) {
            prediction += phi[i] * theta[i];
        }
        const double error = xNext - prediction;

        for (int i = 0; i < 3; ++i) {
            theta[i] += gain[i] * error;
        }

        std::array<std::array<double, 3>, 3> updated{};
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                double correction = 0.0;
                for (int m = 0; m < 3; ++m) {
                    correction += gain[i] * phi[m] * covariance[m][j];
                }
                updated[i][j] =
                    (covariance[i][j] - correction) / forgettingFactor;
            }
        }
        covariance = updated;

        theta[0] = clampValue(theta[0], 0.60, 1.08);
        theta[1] = clampValue(theta[1], 0.08, 0.50);
        theta[2] = clampValue(theta[2], -0.20, 0.20);
    }

    const std::array<double, 3>& parameters() const {
        return theta;
    }

private:
    std::array<double, 3> theta;
    std::array<std::array<double, 3>, 3> covariance;
    double forgettingFactor;
};

bool modelIsCertifiable(const std::array<double, 3>& model) {
    return model[0] >= 0.60 && model[0] <= 1.08
        && model[1] >= 0.08 && model[1] <= 0.50
        && std::abs(model[2]) <= 0.20;
}

int main() {
    constexpr int steps = 300;
    constexpr int supervisoryPeriod = 5;
    constexpr int horizon = 10;
    constexpr double localFeedbackGain = 1.8;

    double x = 0.0;
    double previousInput = 0.0;
    double governedReference = 0.0;

    ProjectedRls rls;
    std::array<double, 3> certifiedModel = rls.parameters();
    std::vector<double> plannedInput(horizon, 0.0);
    std::vector<double> plannedState(horizon + 1, 0.0);

    std::ofstream csv("Chapter27_Lesson4_results_cpp.csv");
    if (!csv) {
        throw std::runtime_error("Cannot open CSV output file.");
    }
    csv << "k,x,u,reference,price,a_hat,b_hat,d_hat,a_cert,b_cert,d_cert\n";
    csv << std::setprecision(12);

    double squaredErrorSum = 0.0;
    double maximumAbsoluteState = 0.0;

    for (int k = 0; k < steps; ++k) {
        const double price = 1.0 + 0.35 * std::sin(2.0 * PI * k / 90.0);
        const double demand = (k < 210) ? 2.4 : 1.6;

        if (k % supervisoryPeriod == 0) {
            const double economicReference =
                5.0 * demand / (5.0 + 0.7 * price);
            governedReference += clampValue(
                economicReference - governedReference, -0.18, 0.18);

            if (modelIsCertifiable(rls.parameters())) {
                certifiedModel = rls.parameters();
            }

            std::vector<double> warmStart(horizon);
            for (int i = 0; i < horizon - 1; ++i) {
                warmStart[i] = plannedInput[i + 1];
            }
            warmStart[horizon - 1] = plannedInput[horizon - 1];

            const MpcPlan plan = solveSupervisoryMpc(
                x,
                certifiedModel,
                governedReference,
                price,
                previousInput,
                warmStart,
                horizon);
            plannedInput = plan.input;
            plannedState = plan.state;
        }

        const int planIndex = std::min(k % supervisoryPeriod, horizon - 1);
        const double nominalInput = plannedInput[planIndex];
        const double nominalState = plannedState[planIndex];
        const double u = clampValue(
            nominalInput + localFeedbackGain * (nominalState - x),
            -2.0,
            2.0);

        const double aTrue = (k < 150) ? 0.92 : 0.98;
        const double bTrue = 0.22;
        const double dTrue = 0.03 + 0.01 * std::sin(2.0 * PI * k / 50.0);
        const double xNext = aTrue * x + bTrue * u + dTrue;

        rls.update(x, u, xNext);
        const auto& estimate = rls.parameters();

        csv << k << ',' << x << ',' << u << ',' << governedReference << ','
            << price << ',' << estimate[0] << ',' << estimate[1] << ','
            << estimate[2] << ',' << certifiedModel[0] << ','
            << certifiedModel[1] << ',' << certifiedModel[2] << '\n';

        squaredErrorSum += std::pow(x - governedReference, 2.0);
        maximumAbsoluteState = std::max(maximumAbsoluteState, std::abs(x));

        x = xNext;
        previousInput = u;
    }

    const double rmse = std::sqrt(squaredErrorSum / steps);
    std::cout << "Saved Chapter27_Lesson4_results_cpp.csv\n";
    std::cout << "Tracking RMSE: " << rmse << '\n';
    std::cout << "Maximum absolute state: " << maximumAbsoluteState << '\n';
    return 0;
}

This version is dependency-free. EJML is suitable for dense/sparse matrix algebra, while ojAlgo can formulate quadratic programs for a larger Java implementation.

Chapter27_Lesson4.java

// Chapter27_Lesson4.java
// Build: javac Chapter27_Lesson4.java
// Run:   java Chapter27_Lesson4

import java.io.BufferedWriter;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Arrays;
import java.util.Locale;

public final class Chapter27_Lesson4 {
    private static final int HORIZON = 10;

    private Chapter27_Lesson4() {}

    private static double clamp(double value, double lower, double upper) {
        return Math.max(lower, Math.min(upper, value));
    }

    private static double[] projectPlan(
            double[] plan,
            double previousInput,
            double inputMin,
            double inputMax,
            double deltaInputMax) {

        double[] projected = new double[plan.length];
        double last = previousInput;
        for (int i = 0; i < plan.length; i++) {
            double lower = Math.max(inputMin, last - deltaInputMax);
            double upper = Math.min(inputMax, last + deltaInputMax);
            projected[i] = clamp(plan[i], lower, upper);
            last = projected[i];
        }
        return projected;
    }

    private static double rolloutCost(
            double[] plan,
            double x0,
            double[] model,
            double reference,
            double price,
            double previousInput) {

        final double qTracking = 4.0;
        final double rEnergy = 0.08;
        final double rMove = 0.50;
        final double qTerminal = 8.0;
        final double statePenalty = 200.0;
        final double stateTightLimit = 3.60;

        double x = x0;
        double lastU = previousInput;
        double total = 0.0;

        for (double u : plan) {
            x = model[0] * x + model[1] * u + model[2];
            double violation = Math.max(0.0, Math.abs(x) - stateTightLimit);
            total += qTracking * Math.pow(x - reference, 2.0)
                    + rEnergy * price * u * u
                    + rMove * Math.pow(u - lastU, 2.0)
                    + statePenalty * violation * violation;
            lastU = u;
        }

        total += qTerminal * Math.pow(x - reference, 2.0);
        return total;
    }

    private static final class MpcPlan {
        private final double[] input;
        private final double[] state;

        private MpcPlan(double[] input, double[] state) {
            this.input = input;
            this.state = state;
        }
    }

    private static MpcPlan solveSupervisoryMpc(
            double x0,
            double[] model,
            double reference,
            double price,
            double previousInput,
            double[] warmStart) {

        double[] plan = Arrays.copyOf(warmStart, HORIZON);
        plan = projectPlan(plan, previousInput, -2.0, 2.0, 0.35);

        double stepSize = 0.08;
        final double finiteDifference = 1.0e-4;

        for (int iteration = 0; iteration < 35; iteration++) {
            double baseCost = rolloutCost(
                    plan, x0, model, reference, price, previousInput);

            double[] gradient = new double[HORIZON];
            double gradientNormSquared = 0.0;

            for (int j = 0; j < HORIZON; j++) {
                double[] perturbed = plan.clone();
                perturbed[j] += finiteDifference;
                perturbed = projectPlan(
                        perturbed, previousInput, -2.0, 2.0, 0.35);
                gradient[j] = (
                        rolloutCost(
                                perturbed,
                                x0,
                                model,
                                reference,
                                price,
                                previousInput)
                        - baseCost
                ) / finiteDifference;
                gradientNormSquared += gradient[j] * gradient[j];
            }

            double[] candidate = new double[HORIZON];
            for (int j = 0; j < HORIZON; j++) {
                candidate[j] = plan[j] - stepSize * gradient[j];
            }
            candidate = projectPlan(
                    candidate, previousInput, -2.0, 2.0, 0.35);

            double candidateCost = rolloutCost(
                    candidate, x0, model, reference, price, previousInput);

            if (candidateCost < baseCost) {
                plan = candidate;
                stepSize = Math.min(0.12, 1.05 * stepSize);
            } else {
                stepSize *= 0.5;
            }

            if (Math.sqrt(gradientNormSquared) < 1.0e-4) {
                break;
            }
        }

        double[] predictedState = new double[HORIZON + 1];
        predictedState[0] = x0;
        for (int i = 0; i < HORIZON; i++) {
            predictedState[i + 1] =
                    model[0] * predictedState[i]
                    + model[1] * plan[i]
                    + model[2];
        }

        return new MpcPlan(plan, predictedState);
    }

    private static final class ProjectedRls {
        private final double forgettingFactor = 0.998;
        private final double[] theta = {0.85, 0.22, 0.0};
        private double[][] covariance = {
                {20.0, 0.0, 0.0},
                {0.0, 20.0, 0.0},
                {0.0, 0.0, 20.0}
        };

        private void update(double x, double u, double xNext) {
            double[] phi = {x, u, 1.0};
            double[] pPhi = new double[3];

            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 3; j++) {
                    pPhi[i] += covariance[i][j] * phi[j];
                }
            }

            double denominator = forgettingFactor;
            for (int i = 0; i < 3; i++) {
                denominator += phi[i] * pPhi[i];
            }

            double[] gain = new double[3];
            for (int i = 0; i < 3; i++) {
                gain[i] = pPhi[i] / denominator;
            }

            double prediction = 0.0;
            for (int i = 0; i < 3; i++) {
                prediction += phi[i] * theta[i];
            }
            double predictionError = xNext - prediction;

            for (int i = 0; i < 3; i++) {
                theta[i] += gain[i] * predictionError;
            }

            double[][] updated = new double[3][3];
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 3; j++) {
                    double correction = 0.0;
                    for (int m = 0; m < 3; m++) {
                        correction += gain[i] * phi[m] * covariance[m][j];
                    }
                    updated[i][j] =
                            (covariance[i][j] - correction)
                            / forgettingFactor;
                }
            }
            covariance = updated;

            theta[0] = clamp(theta[0], 0.60, 1.08);
            theta[1] = clamp(theta[1], 0.08, 0.50);
            theta[2] = clamp(theta[2], -0.20, 0.20);
        }

        private double[] parameters() {
            return theta.clone();
        }
    }

    private static boolean modelIsCertifiable(double[] model) {
        return model[0] >= 0.60 && model[0] <= 1.08
                && model[1] >= 0.08 && model[1] <= 0.50
                && Math.abs(model[2]) <= 0.20;
    }

    public static void main(String[] args) throws IOException {
        Locale.setDefault(Locale.US);

        final int steps = 300;
        final int supervisoryPeriod = 5;
        final double localFeedbackGain = 1.8;

        double x = 0.0;
        double previousInput = 0.0;
        double governedReference = 0.0;

        ProjectedRls rls = new ProjectedRls();
        double[] certifiedModel = rls.parameters();
        double[] plannedInput = new double[HORIZON];
        double[] plannedState = new double[HORIZON + 1];

        double squaredErrorSum = 0.0;
        double maximumAbsoluteState = 0.0;

        try (BufferedWriter writer = new BufferedWriter(
                new FileWriter("Chapter27_Lesson4_results_java.csv"))) {

            writer.write(
                    "k,x,u,reference,price,a_hat,b_hat,d_hat,"
                    + "a_cert,b_cert,d_cert\n");

            for (int k = 0; k < steps; k++) {
                double price =
                        1.0 + 0.35 * Math.sin(2.0 * Math.PI * k / 90.0);
                double demand = (k < 210) ? 2.4 : 1.6;

                if (k % supervisoryPeriod == 0) {
                    double economicReference =
                            5.0 * demand / (5.0 + 0.7 * price);
                    governedReference += clamp(
                            economicReference - governedReference,
                            -0.18,
                            0.18);

                    double[] candidateModel = rls.parameters();
                    if (modelIsCertifiable(candidateModel)) {
                        certifiedModel = candidateModel;
                    }

                    double[] warmStart = new double[HORIZON];
                    for (int i = 0; i < HORIZON - 1; i++) {
                        warmStart[i] = plannedInput[i + 1];
                    }
                    warmStart[HORIZON - 1] =
                            plannedInput[HORIZON - 1];

                    MpcPlan plan = solveSupervisoryMpc(
                            x,
                            certifiedModel,
                            governedReference,
                            price,
                            previousInput,
                            warmStart);
                    plannedInput = plan.input;
                    plannedState = plan.state;
                }

                int planIndex =
                        Math.min(k % supervisoryPeriod, HORIZON - 1);
                double nominalInput = plannedInput[planIndex];
                double nominalState = plannedState[planIndex];
                double u = clamp(
                        nominalInput
                                + localFeedbackGain * (nominalState - x),
                        -2.0,
                        2.0);

                double aTrue = (k < 150) ? 0.92 : 0.98;
                double bTrue = 0.22;
                double dTrue =
                        0.03 + 0.01 * Math.sin(2.0 * Math.PI * k / 50.0);
                double xNext = aTrue * x + bTrue * u + dTrue;

                rls.update(x, u, xNext);
                double[] estimate = rls.parameters();

                writer.write(String.format(
                        Locale.US,
                        "%d,%.12f,%.12f,%.12f,%.12f,"
                        + "%.12f,%.12f,%.12f,%.12f,%.12f,%.12f%n",
                        k,
                        x,
                        u,
                        governedReference,
                        price,
                        estimate[0],
                        estimate[1],
                        estimate[2],
                        certifiedModel[0],
                        certifiedModel[1],
                        certifiedModel[2]));

                squaredErrorSum += Math.pow(x - governedReference, 2.0);
                maximumAbsoluteState =
                        Math.max(maximumAbsoluteState, Math.abs(x));

                x = xNext;
                previousInput = u;
            }
        }

        double rmse = Math.sqrt(squaredErrorSum / steps);
        System.out.printf("Saved Chapter27_Lesson4_results_java.csv%n");
        System.out.printf("Tracking RMSE: %.6f%n", rmse);
        System.out.printf(
                "Maximum absolute state: %.6f%n",
                maximumAbsoluteState);
    }
}

The script runs with base MATLAB. Its local functions map directly to MATLAB Function blocks. When Optimization Toolbox or Model Predictive Control Toolbox is available, replace the teaching optimizer with quadprog, mpc, or nlmpc. The optional final function creates a named Simulink architecture shell.

Chapter27_Lesson4.m

% Chapter27_Lesson4.m
% Hierarchical adaptive control:
% economic target -> supervisory MPC -> fast correction -> plant.
%
% The executable simulation uses base MATLAB only. The projected-gradient
% solver can be replaced by quadprog, nlmpc, or an MPC Toolbox controller.

clear; clc; close all;

steps = 300;
supervisoryPeriod = 5;
horizon = 10;
localFeedbackGain = 1.8;

x = 0.0;
previousInput = 0.0;
governedReference = 0.0;

theta = [0.85; 0.22; 0.0];
P = 20.0 * eye(3);
forgettingFactor = 0.998;
certifiedModel = theta;

plannedInput = zeros(horizon, 1);
plannedState = zeros(horizon + 1, 1);

logData = zeros(steps, 11);

for k = 0:(steps - 1)
    price = 1.0 + 0.35 * sin(2.0 * pi * k / 90.0);
    if k < 210
        demand = 2.4;
    else
        demand = 1.6;
    end

    if mod(k, supervisoryPeriod) == 0
        % min_r 5(r-demand)^2 + 0.7*price*r^2
        economicReference = 5.0 * demand / (5.0 + 0.7 * price);
        governedReference = governedReference + clampValue( ...
            economicReference - governedReference, -0.18, 0.18);

        if modelIsCertifiable(theta)
            certifiedModel = theta;
        end

        warmStart = [plannedInput(2:end); plannedInput(end)];
        [plannedInput, plannedState] = solveSupervisoryMpc( ...
            x, certifiedModel, governedReference, price, ...
            previousInput, warmStart, horizon);
    end

    planIndex = min(mod(k, supervisoryPeriod) + 1, horizon);
    nominalInput = plannedInput(planIndex);
    nominalState = plannedState(planIndex);

    u = clampValue( ...
        nominalInput + localFeedbackGain * (nominalState - x), ...
        -2.0, 2.0);

    if k < 150
        aTrue = 0.92;
    else
        aTrue = 0.98;
    end
    bTrue = 0.22;
    dTrue = 0.03 + 0.01 * sin(2.0 * pi * k / 50.0);
    xNext = aTrue * x + bTrue * u + dTrue;

    phi = [x; u; 1.0];
    gain = P * phi / (forgettingFactor + phi' * P * phi);
    predictionError = xNext - phi' * theta;
    theta = theta + gain * predictionError;
    P = (P - gain * phi' * P) / forgettingFactor;

    theta(1) = clampValue(theta(1), 0.60, 1.08);
    theta(2) = clampValue(theta(2), 0.08, 0.50);
    theta(3) = clampValue(theta(3), -0.20, 0.20);

    logData(k + 1, :) = [ ...
        k, x, u, governedReference, price, ...
        theta', certifiedModel'];

    x = xNext;
    previousInput = u;
end

variableNames = { ...
    'k', 'x', 'u', 'reference', 'price', ...
    'a_hat', 'b_hat', 'd_hat', ...
    'a_cert', 'b_cert', 'd_cert'};
results = array2table(logData, 'VariableNames', variableNames);
writetable(results, 'Chapter27_Lesson4_results_matlab.csv');

rmse = sqrt(mean((results.x - results.reference).^2));
fprintf('Saved Chapter27_Lesson4_results_matlab.csv\n');
fprintf('Tracking RMSE: %.6f\n', rmse);
fprintf('Maximum absolute state: %.6f\n', max(abs(results.x)));

figure;
plot(results.k, results.x, 'LineWidth', 1.2); hold on;
plot(results.k, results.reference, '--', 'LineWidth', 1.2);
grid on; xlabel('sample'); ylabel('state');
legend('state', 'governed reference');

figure;
plot(results.k, results.u, 'LineWidth', 1.2);
grid on; xlabel('sample'); ylabel('input');

figure;
plot(results.k, results.a_hat, 'LineWidth', 1.2); hold on;
xline(150, '--');
grid on; xlabel('sample'); ylabel('estimated a');
legend('a estimate', 'plant change');

% Optional: create a Simulink architecture shell whose subsystems correspond
% to the functions used above. Set this flag to true when Simulink is present.
BUILD_SIMULINK_SHELL = false;
if BUILD_SIMULINK_SHELL && license('test', 'Simulink')
    buildSimulinkShell();
end

function value = clampValue(value, lower, upper)
    value = max(lower, min(upper, value));
end

function accepted = modelIsCertifiable(model)
    accepted = model(1) >= 0.60 && model(1) <= 1.08 && ...
               model(2) >= 0.08 && model(2) <= 0.50 && ...
               abs(model(3)) <= 0.20;
end

function projected = projectPlan( ...
    plan, previousInput, inputMin, inputMax, deltaInputMax)

    projected = zeros(size(plan));
    last = previousInput;
    for i = 1:numel(plan)
        lower = max(inputMin, last - deltaInputMax);
        upper = min(inputMax, last + deltaInputMax);
        projected(i) = clampValue(plan(i), lower, upper);
        last = projected(i);
    end
end

function total = rolloutCost( ...
    plan, x0, model, reference, price, previousInput)

    qTracking = 4.0;
    rEnergy = 0.08;
    rMove = 0.50;
    qTerminal = 8.0;
    statePenalty = 200.0;
    stateTightLimit = 3.60;

    x = x0;
    lastInput = previousInput;
    total = 0.0;

    for i = 1:numel(plan)
        x = model(1) * x + model(2) * plan(i) + model(3);
        violation = max(0.0, abs(x) - stateTightLimit);
        total = total + ...
            qTracking * (x - reference)^2 + ...
            rEnergy * price * plan(i)^2 + ...
            rMove * (plan(i) - lastInput)^2 + ...
            statePenalty * violation^2;
        lastInput = plan(i);
    end

    total = total + qTerminal * (x - reference)^2;
end

function [plan, predictedState] = solveSupervisoryMpc( ...
    x0, model, reference, price, previousInput, warmStart, horizon)

    plan = warmStart(1:horizon);
    plan = projectPlan(plan, previousInput, -2.0, 2.0, 0.35);

    stepSize = 0.08;
    finiteDifference = 1.0e-4;

    for iteration = 1:35
        baseCost = rolloutCost( ...
            plan, x0, model, reference, price, previousInput);

        gradient = zeros(horizon, 1);
        for j = 1:horizon
            perturbed = plan;
            perturbed(j) = perturbed(j) + finiteDifference;
            perturbed = projectPlan( ...
                perturbed, previousInput, -2.0, 2.0, 0.35);
            gradient(j) = ( ...
                rolloutCost( ...
                    perturbed, x0, model, reference, ...
                    price, previousInput) - baseCost) ...
                / finiteDifference;
        end

        candidate = projectPlan( ...
            plan - stepSize * gradient, ...
            previousInput, -2.0, 2.0, 0.35);
        candidateCost = rolloutCost( ...
            candidate, x0, model, reference, price, previousInput);

        if candidateCost < baseCost
            plan = candidate;
            stepSize = min(0.12, 1.05 * stepSize);
        else
            stepSize = 0.5 * stepSize;
        end

        if norm(gradient) < 1.0e-4
            break;
        end
    end

    predictedState = zeros(horizon + 1, 1);
    predictedState(1) = x0;
    for i = 1:horizon
        predictedState(i + 1) = ...
            model(1) * predictedState(i) + ...
            model(2) * plan(i) + model(3);
    end
end

function buildSimulinkShell()
    model = 'Chapter27_Lesson4_SupervisoryShell';
    if bdIsLoaded(model)
        close_system(model, 0);
    end
    new_system(model);
    open_system(model);

    add_block('simulink/Ports & Subsystems/Subsystem', ...
        [model '/Economic_Target_and_Reference_Governor'], ...
        'Position', [40 70 245 130]);
    add_block('simulink/Ports & Subsystems/Subsystem', ...
        [model '/Supervisory_MPC'], ...
        'Position', [300 70 470 130]);
    add_block('simulink/Ports & Subsystems/Subsystem', ...
        [model '/Fast_Local_Correction'], ...
        'Position', [530 70 720 130]);
    add_block('simulink/Ports & Subsystems/Subsystem', ...
        [model '/Plant'], ...
        'Position', [780 70 900 130]);
    add_block('simulink/Ports & Subsystems/Subsystem', ...
        [model '/Projected_RLS_and_Model_Gate'], ...
        'Position', [360 220 610 280]);

    add_line(model, ...
        'Economic_Target_and_Reference_Governor/1', ...
        'Supervisory_MPC/1', 'autorouting', 'on');
    add_line(model, ...
        'Supervisory_MPC/1', ...
        'Fast_Local_Correction/1', 'autorouting', 'on');
    add_line(model, ...
        'Fast_Local_Correction/1', ...
        'Plant/1', 'autorouting', 'on');

    set_param(model, 'StopTime', '300');
    save_system(model);
    fprintf('Saved %s.slx architecture shell.\n', model);
end

The executable Wolfram Language implementation is stored in a .wl file. The accompanying notebook loads that file from the notebook directory. For larger convex formulations, QuadraticOptimization, ConvexOptimization, or NMinimize can replace the teaching solver.

Chapter27_Lesson4.wl

(* Chapter27_Lesson4.wl
   Hierarchical adaptive control:
   economic target -> supervisory MPC -> fast correction -> plant.
*)

ClearAll["Global`*"];

ClampValue[value_, lower_, upper_] := Max[lower, Min[upper, value]];

ProjectPlan[plan_List, previousInput_, inputMin_ : -2.0,
   inputMax_ : 2.0, deltaInputMax_ : 0.35] :=
 Module[{projected = ConstantArray[0.0, Length[plan]], last = previousInput,
   lower, upper},
  Do[
   lower = Max[inputMin, last - deltaInputMax];
   upper = Min[inputMax, last + deltaInputMax];
   projected[[i]] = ClampValue[plan[[i]], lower, upper];
   last = projected[[i]],
   {i, Length[plan]}];
  projected
 ]

RolloutCost[plan_List, x0_, model_List, reference_, price_,
   previousInput_] :=
 Module[{qTracking = 4.0, rEnergy = 0.08, rMove = 0.50,
   qTerminal = 8.0, statePenalty = 200.0, stateTightLimit = 3.60,
   x = x0, lastU = previousInput, total = 0.0, violation},
  Do[
   x = model[[1]] x + model[[2]] u + model[[3]];
   violation = Max[0.0, Abs[x] - stateTightLimit];
   total += qTracking (x - reference)^2 +
     rEnergy price u^2 + rMove (u - lastU)^2 +
     statePenalty violation^2;
   lastU = u,
   {u, plan}];
  total + qTerminal (x - reference)^2
 ]

SolveSupervisoryMPC[x0_, model_List, reference_, price_,
   previousInput_, warmStart_List, horizon_ : 10] :=
 Module[{plan, stepSize = 0.08, finiteDifference = 1.0*^-4,
   baseCost, gradient, perturbed, candidate, candidateCost,
   predictedState},
  plan = ProjectPlan[Take[PadRight[warmStart, horizon, previousInput],
     horizon], previousInput];

  Do[
   baseCost = RolloutCost[plan, x0, model, reference, price,
     previousInput];
   gradient = Table[
     perturbed = plan;
     perturbed[[j]] += finiteDifference;
     perturbed = ProjectPlan[perturbed, previousInput];
     (RolloutCost[perturbed, x0, model, reference, price,
         previousInput] - baseCost)/finiteDifference,
     {j, horizon}];

   candidate = ProjectPlan[plan - stepSize gradient, previousInput];
   candidateCost = RolloutCost[candidate, x0, model, reference,
     price, previousInput];

   If[candidateCost < baseCost,
    plan = candidate; stepSize = Min[0.12, 1.05 stepSize],
    stepSize *= 0.5];

   If[Norm[gradient] < 1.0*^-4, Break[]],
   {35}];

  predictedState = FoldList[
    model[[1]] #1 + model[[2]] #2 + model[[3]] &,
    x0, plan];
  {plan, predictedState}
 ]

ModelIsCertifiable[model_List] :=
 0.60 <= model[[1]] <= 1.08 &&
  0.08 <= model[[2]] <= 0.50 &&
  Abs[model[[3]]] <= 0.20;

steps = 300;
supervisoryPeriod = 5;
horizon = 10;
localFeedbackGain = 1.8;

x = 0.0;
previousInput = 0.0;
governedReference = 0.0;
theta = {0.85, 0.22, 0.0};
covariance = 20.0 IdentityMatrix[3];
forgettingFactor = 0.998;
certifiedModel = theta;
plannedInput = ConstantArray[0.0, horizon];
plannedState = ConstantArray[0.0, horizon + 1];
records = {};

Do[
 price = 1.0 + 0.35 Sin[2.0 Pi k/90.0];
 demand = If[k < 210, 2.4, 1.6];

 If[Mod[k, supervisoryPeriod] == 0,
  economicReference = 5.0 demand/(5.0 + 0.7 price);
  governedReference += ClampValue[
    economicReference - governedReference, -0.18, 0.18];

  If[ModelIsCertifiable[theta], certifiedModel = theta];

  warmStart = Join[Rest[plannedInput], {Last[plannedInput]}];
  {plannedInput, plannedState} =
   SolveSupervisoryMPC[x, certifiedModel, governedReference,
    price, previousInput, warmStart, horizon];
  ];

 planIndex = Min[Mod[k, supervisoryPeriod] + 1, horizon];
 nominalInput = plannedInput[[planIndex]];
 nominalState = plannedState[[planIndex]];
 u = ClampValue[
   nominalInput + localFeedbackGain (nominalState - x), -2.0, 2.0];

 aTrue = If[k < 150, 0.92, 0.98];
 bTrue = 0.22;
 dTrue = 0.03 + 0.01 Sin[2.0 Pi k/50.0];
 xNext = aTrue x + bTrue u + dTrue;

 phi = {x, u, 1.0};
 gain = covariance.phi/
   (forgettingFactor + phi.covariance.phi);
 predictionError = xNext - phi.theta;
 theta = theta + gain predictionError;
 covariance = (covariance -
     Outer[Times, gain, phi].covariance)/forgettingFactor;

 theta[[1]] = ClampValue[theta[[1]], 0.60, 1.08];
 theta[[2]] = ClampValue[theta[[2]], 0.08, 0.50];
 theta[[3]] = ClampValue[theta[[3]], -0.20, 0.20];

 AppendTo[records,
  Join[{k, x, u, governedReference, price}, theta, certifiedModel]];

 x = xNext;
 previousInput = u,
 {k, 0, steps - 1}];

headers = {"k", "x", "u", "reference", "price", "a_hat", "b_hat",
   "d_hat", "a_cert", "b_cert", "d_cert"};
Export["Chapter27_Lesson4_results_wolfram.csv",
 Prepend[records, headers]];

rmse = Sqrt[Mean[(records[[All, 2]] - records[[All, 4]])^2]];
Print["Tracking RMSE: ", N[rmse, 8]];
Print["Maximum absolute state: ", Max[Abs[records[[All, 2]]]]];

ListLinePlot[
 {records[[All, {1, 2}]], records[[All, {1, 4}]]},
 PlotLegends -> {"state", "governed reference"},
 Frame -> True, FrameLabel -> {"sample", "state"}]

Chapter27_Lesson4.nb

Notebook[{
 Cell["Chapter 27, Lesson 4: Interaction with Supervisory Layers", "Title"],
 Cell["This notebook loads and executes Chapter27_Lesson4.wl from the same directory.", "Text"],
 Cell[BoxData[
  RowBox[{"Get", "[",
   RowBox[{"FileNameJoin", "[",
    RowBox[{"{",
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], ",",
      "\"Chapter27_Lesson4.wl\""}],
     "}"}],
    "]"}],
   "]"}]], "Input"]
},
WindowTitle -> "Chapter27_Lesson4",
StyleDefinitions -> "Default.nb"]

Problem 1 (Authority allocation): A hard actuator satisfies \(|u|\le 10\). The disturbance reserve is 1.5 and the local adaptive correction can demand at most 2.0. Find the largest symmetric supervisory input bound.

Solution: The Minkowski-sum budget requires \(\bar u_{\mathrm{sup}}+2.0+1.5\le 10\). Therefore \(\bar u_{\mathrm{sup}}\le 6.5\), so the MPC must use \(\mathcal{U}_{\mathrm{sup}}=[-6.5,6.5]\).

Problem 2 (Economic target): For \(\ell(r)=q_d(r-r_d)^2+c_ppr^2\), derive the unconstrained optimum and explain its limiting behavior as price grows.

Solution:

\[ \frac{d\ell}{dr} =2q_d(r-r_d)+2c_ppr=0 \quad\Longrightarrow\quad r^\star=\frac{q_d}{q_d+c_pp}r_d. \]

As \(p\) increases, the denominator grows and the economically selected target moves toward zero. With state constraints, project this value onto the admissible steady-state set.

Problem 3 (RLS projection): Suppose unconstrained RLS produces \(\hat b^-=-0.03\), while physical knowledge gives \(b\in[0.08,0.50]\). What value is passed to the model gate, and why is this not enough to certify the model?

Solution: Euclidean interval projection gives \(\hat b=0.08\). Projection enforces prior bounds but does not create information. The covariance, residual validation, excitation, and resulting MPC feasibility must still be checked.

Problem 4 (loss of recursive feasibility): A sequence is feasible for \(x^+=0.9x+0.2u\). At the next update the accepted model becomes \(x^+=1.05x+0.2u\). Explain why shifting the old sequence is not a proof of feasibility.

Solution: Every predicted state changes because the state-transition multiplier changed. A trajectory formerly inside \(\mathcal{X}\) may leave it, and its terminal state may leave \(\mathcal{X}_f\). The standard shift argument applies only for compatible dynamics. Robust tightening, bounded model updates, or an explicit feasibility gate is required.

Problem 5 (composite stability): Given \(\Delta V_a\le-3\|e\|^2+\delta_a\) and \(\Delta V_N\le-2\|z\|^2+4\|e\|+\delta_N\), form \(W=V_N+cV_a\) with \(c=1\). Use Young's inequality with \(\eta=2\).

Solution: Since \(4\|e\|\le\|e\|^2+4\),

\[ \Delta W \le -2\|z\|^2-2\|e\|^2+ \delta_a+\delta_N+4. \]

Hence the hierarchy is practically stable: trajectories enter a residual set whose size depends on the additive bound \(\delta_a+\delta_N+4\). Tighter coupling bounds reduce that residual set.

Problem 6 (multi-rate design): A local loop runs at 1 kHz, the supervisory MPC at 20 Hz, and the economic optimizer every 30 s. How many local samples occur per MPC update, and how many MPC updates occur per economic update?

Solution: The MPC period is 0.05 s, so there are \(1000/20=50\) local samples per MPC update. In 30 s, the MPC performs \(20\times30=600\) updates. Software interfaces must therefore avoid blocking the 1 kHz task while the slower layers solve optimization problems.

A successful adaptive/MPC hierarchy requires more than connecting an estimator to an optimizer. The design must explicitly define time scales, certified model hand-off, target governance, actuator reserves, constraint tightening, local correction limits, solver fallback, and interconnection stability. The adaptive layer handles fast uncertainty and mismatch; supervisory MPC coordinates constraints and trajectories; the optimization layer chooses economically meaningful operating points. Practical stability follows when coupling signals, model changes, reference motion, and disturbances are bounded and the local and supervisory decrease conditions dominate those couplings.