Current-Harnessing Stage-Gated MPC

• Current-Harnessing Stage-Gated MPC is a control technique that uses monotonic cost modifications to enhance stability, constraint satisfaction, and performance in model predictive controllers.
• It employs stagewise, increasing weight profiles to replace traditional terminal constraints, ensuring a systematic closed-loop improvement and robust energy efficiency.
• Practical applications include autonomous underwater vehicles and LQR systems, where careful tuning of weight exponents and smooth cost functions provides design flexibility and guaranteed stability.

Monotone Cost Shaping (MCS) is a methodology for modifying the objective of optimal control and model predictive control (MPC) schemes such that performance, stability, or design objectives are achieved via stagewise, monotonic cost modifications—without degrading the original objective. The core principle is replacing or augmenting standard cost functions with monotonic weight profiles or term modifications, thereby enforcing desirable closed-loop properties including stability, constraint-satisfaction, and design flexibility. MCS has emerged in nonlinear MPC as an alternative to terminal constraints and in continuous and discrete linear-quadratic regulation (LQR) as a tool for systematic performance index shaping, and supports plug-and-play inclusion in advanced domain-specific controllers such as ocean current-assisted vehicle path planners.

1. Monotone Cost Shaping in Finite-Horizon Nonlinear MPC

The seminal MCS approach for nonlinear MPC bypasses terminal-state constraints and terminal costs by embedding a time-varying, polynomial-type monotonic weight profile in the stage cost. The system dynamics are $x_{k+1} = f(x_k, u_k)$ with a continuous, positive-definite stage cost $\ell(x)$. Given a prediction horizon $N$ satisfying $0$-reachability (i.e., the origin is $N$-step reachable), and a weight exponent $m \in \mathbb{N}$, the per-stage weights are defined as $w_i = (i/N)^m$ for $i=1,\ldots,N$, leading to the finite-horizon cost

$J_m(u|x_k) = \sum_{i=1}^N w_i\,\ell(x_u^i(x_k)),$

where $x_u^i$ denotes the $i$-step state under input sequence $u$. The MCS-MPC scheme solves

$$u^*(x_k,m) = \arg\min_{u\in\mathcal{U}^N} J_m(u|x_k),$$

applies $u_0^*$, then repeats.

Key properties are enforced by the weight profile:

• Monotonicity: $w_{i+1} \geq w_i$ for all $i$.
• Exponential growth: $w_{i+1}/w_i \geq ((i+1)/i)^m$, with the tail penalty increasing rapidly as $m$ grows.

System assumptions consist of:

• N-step reachability: The origin is reachable in $N$ steps from the considered compact set.
• Local control-Lyapunov property: Local stage decrease guarantees via a Lyapunov-type argument.

Main stability result: Under these assumptions, for $m$ exceeding an explicit threshold, the closed-loop is asymptotically stable on the compact set, with the value function $V(x) = J_m^*(x)$ serving as a Lyapunov function satisfying a uniform decrease. Explicit bounds on $m$ ensure applicability for a given system and cost (Alamir, 2017).

2. Theoretical Guarantees and Proof Outline

The central stability proposition is that, for large enough $m$,

$$V(f(x,u_0^*)) - V(x) \leq -\frac{1}{N^m} \ell(x_1) - \frac{\gamma}{2} \ell(x_N^*(x)),$$

so $V(x_{k+1}) \leq V(x_k) - \alpha(\|x_k\|)$ for some positive definite $\alpha$. The proof constructs a tail-state bound $\ell(x_N^*) \leq \eta\,c^m$ with $c = (N-1)/N$, estimates the effect of shifted input, and ensures, via monotonicity of the weights, that the Lyapunov decrease outpaces all residual terms for sufficiently large $m$.

Explicit tuning rules are as follows: Select $N$ for which $0$ is $N$-step reachable, estimate $\eta$ reflecting the cost along admissible trajectories, and set $m$ to satisfy

$$m \geq \max\left\{ \frac{\ln(\bar{\rho}/\eta)}{\ln c},\, \frac{\ln(\gamma/2)}{\ln c} \right\}.$$

This ensures both tail-state control and sufficient Lyapunov descent (Alamir, 2017).

3. Monotone Cost Shaping in Performance Index Design and LQR

MCS generalizes to the design of performance indices for closed-loop optimal control, including infinite-horizon LQR and nonlinear control-affine systems. For continuous-time LQR, a monotone shaping term

$\phi(x,u) = \frac{1}{2} \begin{pmatrix}x\u\end{pmatrix}^T H \begin{pmatrix}x\u\end{pmatrix},\quad H \succeq 0$

is appended to the nominal cost with scaling $\alpha \geq 0$, yielding a new LQR problem with effective weights $Q(\alpha)$, $R(\alpha)$, and $M(\alpha)$ parameterized by $\alpha$. The optimal feedback gain $K(\alpha)$ is then: $$K(\alpha) = \left[R(\alpha) + \gamma B^T P(\alpha) B\right]^{-1} \left[\gamma B^T P(\alpha)A + M(\alpha)^T\right].$$ Importantly, under positivity of $H$, $K(\alpha)$ is monotonically nondecreasing in the Loewner order with respect to $\alpha$. Extension to nonlinear control-affine systems yields an update law $$u_p^*(x) = u_0^*(x) - \frac{1}{2} R^{-1} g(x)^T \nabla_x h(x,\theta)$$, with $h$ constructed as the solution to a linear PDE ensuring the desired trajectory-level shaping effect (Rai et al., 11 Oct 2025).

4. Monotone Cost Shaping in Energy-Optimal Ocean-Current-Aware Control

A domain-specialized application of MCS appears in staged-gated MPC for autonomous underwater vehicles navigating in realistic, time-varying ocean current fields. Here, MCS takes the form of subtractive, help-gated cost modifications that:

• Relax along-track position errors proportionally to a per-stage "helpfulness" scalar $s_k$ reflecting current alignment and strength;
• Provide a strictly bounded translational energy rebate, also modulated by $s_k$;
• Ensure that, for all feasible trajectories, the total cost after shaping cannot exceed the baseline objective.

For state $x_k$, control $u_k$, and goal $x_g$, the stage cost modification at step $k$ is

$$\Delta \ell_k^{\mathrm{MCS}} = - s_k \left[ \lambda_{\mathrm{relax}} \|e_{\parallel,k}\|^2_{Q_{\mathrm{pos}}} + w_{\mathrm{reb}} \, \varphi(u_{k,\mathrm{lin}})\right],$$

where $e_{\parallel,k}$ is the along-track projection, $w_{\mathrm{reb}}$ is a maximum rebate parameter, and $\varphi(\cdot)$ is a saturation-shaped rebate function in the translational input norm. This stage-cost shaping term satisfies $\Delta \ell_k^{\mathrm{MCS}} \leq 0$ under all circumstances, and the resulting shaped objective $J$ is always $\leq J_{\mathrm{base}}$.

Embedded within a larger predictive control framework, these monotonic shaping terms enable energy-aware navigation with provable constraint satisfaction and plug-and-play smoothness suitable for modern nonlinear programming solvers (Syntakas et al., 31 Jan 2026).

5. Properties, Algorithmic Aspects, and Practical Tuning

Monotone Cost Shaping possesses several distinguishing properties:

• Non-worsening: For subtractive or strictly nonnegative shaping, MCS cannot increase the baseline objective.
• Continuity and smoothness: When implemented via smooth functions (e.g., $\sqrt{\cdot+\epsilon}$, $\tanh(\cdot)$, rational saturations), all optimization gradients are globally $C^1$.
• Parameterization and tuning: MCS instances expose explicit parameters—weight exponents ($m$), penalty fractions ($\lambda_{\mathrm{relax}}$), and utility scalars—enabling direct adjustment for stability or design criteria.

Practical tuning involves:

• Determining the minimal prediction horizon $N$ for reachability;
• Estimating cost and dynamic constants ($\eta$, $\bar{\rho}$, etc.);
• Selecting weight growth/exponents to meet explicit lower bounds derived from Lyapunov feasibility;
• For domain control (e.g., AUVs), calibrating gate sharpness, rebate maxima, and trajectory alignment thresholds using ocean current models and task tolerances (Alamir, 2017, Syntakas et al., 31 Jan 2026).

6. Extensions, Guarantees, and Generalizations

MCS generalizes to a wide array of control-theoretic settings:

• Linear and nonlinear systems: Guaranteed monotonic improvement and non-increase in closed-loop cost under suitable definiteness conditions; explicit updates for both cases.
• Stability and ISS: For nonlinear affine-in-control systems, global asymptotic stability (GAS) and input-to-state stability (ISS) are guaranteed by proper construction of the shaped value function and associated feedback law (Rai et al., 11 Oct 2025).
• Trajectory-level objective optimization: Coupling MCS with differentiable trajectory optimization and parameter-tuning yields gradient-based algorithms for monotonically decreasing trajectory metrics without the need for repeated nonlinear program (NLP) solutions.

The following table summarizes salient theoretical guarantees established in the recent literature:

Setting	Guarantee Type	Reference
Nonlinear NMPC, monotone polynomial weights	Asymptotic stability (Lyapunov)	(Alamir, 2017)
Infinite-horizon LQR, quadratic shaping	Monotonic nondecrease in gain	(Rai et al., 11 Oct 2025)
Nonlinear affine-in-control	GAS, ISS under shaped feedback	(Rai et al., 11 Oct 2025)
AUV energy-optimal MPC	Non-worsening, bounded rebate	(Syntakas et al., 31 Jan 2026)

A plausible implication is that the methodology of monotone cost shaping provides a unifying framework for stability and constraint handling in both traditional (finite-horizon) MPC and modern differentiable control design, obviating the need for restrictive or difficult-to-design terminal penalties in many settings.

7. Implementation and Applications

MCS has been adopted as a plug-and-play module in state-of-the-art nonlinear MPC toolchains. For generic NMPC, implementation requires only the stage-wise replacement $\ell_i \mapsto w_i \ell_i$, with precomputed weight profiles. In ocean current exploitation, the MCS term is combined with stage-gated "helpfulness" functions and may operate alongside additional cost-shaping components such as speed-to-fly (STF) penalties for further task enrichment (Syntakas et al., 31 Jan 2026).

Notably, all cost modifications via MCS are $C^1$-smooth and directly compatible with contemporary direct transcription and NLP solvers such as CasADi+IPOPT. In practice, MCS alone is sufficient to guarantee asymptotic stability and non-worsening design objectives under mild system assumptions, enabling safe, efficient, and robust MPC deployable in domains ranging from robotics to energy systems and beyond.