Add a disturbance-control cross term Sd to the objective

[baggepinnen]

Motivation

With reference tracking and a direct penalty u' R u, a persistent disturbance that has to be rejected causes a steady-state error: the required steady-state input is nonzero, so the optimizer trades tracking error against the u penalty and settles at an offset (mpc2mpqp already warns about this for a nonzero operating point). This happens even when the disturbance is measured and enters the prediction exactly through Bd, and switching to Rr (a penalty on Δu) is not always what one wants.

The standard fix from LQ theory is a cross term between the disturbance and the control (see e.g. the SQ term in the RobustAndOptimalControl.jl LQG disturbance example): with Sd = R and a disturbance entering like the input, Bd = B,

u' R u + 2 d' R u = (u + d)' R (u + d) − d' R d

so the control is penalized relative to the input that cancels the disturbance instead of relative to zero, and the offset disappears while R stays on u.

Changes

• MPCWeights gets a field Sd (nd × nu, empty = zero), settable through set_objective!(mpc; Sd) for both the single- and multi-objective methods. A vector is interpreted as a diagonal, like the other weights.
• Objective ∑ₖ dₖ' Sd uₖ:
  • without disturbance preview d is part of the extended state, so Sd replaces the zero padding of the disturbance rows of S in create_extended_cost;
  • with disturbance preview the dₖ are QP parameters, so the term enters f_theta on the disturbance-preview columns (disturbance_preview_cross_term), honouring uₖ = u_{Nc−1} for k ≥ Nc.
• Sd is validated against the model's current nd (which includes channels added by an offset-free observer) when the QP is formed, with an informative ArgumentError.
• Multiple objectives + disturbance preview + nonzero Sd throws (not supported; the no-preview case works through create_extended_cost).
• evaluate_cost accepts the disturbance trajectory ds and Sd.
• Manual (objective.md) and the set_objective! docstring describe the term.

Tests

New testset "Disturbance-control cross term Sd" (double integrator, Bd = B):

• exact equivalence (1e-8) of the Sd = R problem with the disturbance-free problem in ũ = u + d for {no preview, preview} × {Nc = Np, Nc < Np}, including a time-varying preview trajectory;
• closed-loop steady-state error under a constant disturbance: 3e-2 without Sd, < 1e-9 with Sd = R, both preview modes;
• size validation and evaluate_cost.

The full suite passes locally on Julia 1.12.7.

Verified downstream as well: in a Dyad/ModelingToolkit closed loop (JuliaComputing/DiscreteComponents LinearMPCController, reference + disturbance preview, Np = 20) the same controller goes from a −0.025 offset to 1e-10 with Sd = R.

🤖 Generated with Claude Code

https://claude.ai/code/session_01LNTSoj8uSmNpSqpRTor8di

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

✅ All modified and coverable lines are covered by tests.
✅ Project coverage is 89.66%. Comparing base (ab9340f) to head (738fce9).
⚠️ Report is 1 commits behind head on main.
❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #114      +/-   ##
==========================================
+ Coverage   89.57%   89.66%   +0.09%     
==========================================
  Files          15       15              
  Lines        2389     2410      +21     
==========================================
+ Hits         2140     2161      +21     
  Misses        249      249              

☔ View full report in Codecov by Harness.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.