MPC reference tracking is only valid for equilibrium references

[gabrielfrasantos]

Severity: high
Domain: controllers
Status: VERIFIED — read + hand-traced against a210d34 on 2026-08-10
Suggested labels: bug, numerical-correctness, controllers

Summary

Mpc::ComputeControl() implements reference tracking by substituting state − reference into a
gradient matrix that was precomputed for regulation to the origin. That substitution is only
valid when the reference is a plant equilibrium; for any other reference the resulting control is
wrong.

Location

numerical/controllers/implementations/Mpc.hpp

Evidence

auto effectiveState = reference ? state - *reference : state;
auto g = gradientMatrix * effectiveState;
auto negG = g * T(-1.0f);
auto uOptimal = solvers::SolveSystem<T, TotalControlDim, 1>(hessian, negG);

gradientMatrix is built once as θᵀ Q̄ Ψ:

hessian        = thetaT * qBar * theta + rBar;
gradientMatrix = thetaT * qBar * psi;

This encodes the cost of driving the state to zero, not to reference.

Trace

Scalar plant A = 0, B = 1, horizon 1, Q = R = 1, x = 0, r = 1.

predicted output:  x⁺ = A·x + B·u = u
cost:              (x⁺ − r)² + u²  =  (u − 1)² + u²
minimum:           2(u − 1) + 2u = 0  =>  u = 0.5

code: effectiveState = x − r = −1
      g = gradientMatrix · (−1) ; with A = 0 the gradient term vanishes
      => u = 0

r = 1 is not an equilibrium of A = 0 (A·r = 0 ≠ r), so the shift is invalid.

Suggested fix

Either

1. compute an explicit steady-state target pair (x_ss, u_ss) from
   [[A − I, B], [C, D]] · [x_ss; u_ss] = [0; r] and regulate x − x_ss with an offset u_ss; or
2. reformulate in Δu (velocity form) with the reference entering the tracking error directly.

Add a test asserting the analytic optimum u = 0.5 for the case above.

Notes

Related: issue 009 (constraint handling). Both live in ComputeControl().