betaFeedback

WorksEngineering

socx: GPU-Accelerated Chance-Constrained Stochastic Optimal Control using JAX

Abstract

In this article, we present socx, a GPU-accelerated solver for lifted convex reformulations of linear stochastic optimal control under chance constraints, implemented in JAX. We demonstrate that by exploiting the sparse and independent structure of the problem, distributed optimization can reduce it to simpler problems: independent linear-quadratic tracking (LQT) problems and Euclidean projections onto sets. These simpler problems can be solved in parallel and then coupled using the constraints. To validate our solver, we run it on a hover-linearized 3D quadrotor and show a speed-up of up to 98×98\times over MOSEK and up to 2224×2224\times over SCS. socx also solves horizons up to N=1120N=1120, whereas MOSEK runs out of memory at N=320N=320. We present ablations to isolate the key components that enable the speed-up.

Introduction

Motion planning is a fundamental problem in robotics, as it requires finding an optimal path from the initial to the final state that minimizes length or control effort, is feasible, and avoids obstacles [1]. Autonomous systems such as unmanned aerial vehicles and spacecrafts, face uncertainties, e.g., wrench perturbations, estimation noise, during tasks like safe exploration [2, 3]. Therefore, rather than a single path, the problem becomes one of finding a distribution over paths such that the probability of failure is bounded [4, 5].

Stochastic motion planning is an example of stochastic optimal control, and covariance steering solves such problems by designing feedback to control the state distribution, specifically the mean and covariance, from an initial to a final distribution in a finite time horizon. Covariance steering has many applications in robotics and aerospace. However, to the best of the authors’ knowledge, analytically optimal results are only known for unconstrained cases [6]. For the constrained cases, numerical optimization methods exist that use feedback parameterization, yielding a nonconvex optimization problem with O(Nnx(nu+nx))O(Nn_x(n_u + n_x)) variables. Computationally efficient formulations approximate the constraints, thereby shrinking the feasible set and yielding suboptimal solutions [7, 8]. Recent developments in lifted representations yield problems that can exactly represent linear chance [9] and terminal covariance constraints, with convex approximations for quadratic chance constraints [10], but are computationally prohibitive as the number of variables grows as O(N2nu(nu+nx))O(N^2n_u(n_u + n_x)).

Parallelization introduces a new dimension to algorithm design, enabling new problems to be solved in real time. Batching refers to solving multiple instances of the same problem [11, 12, 13]; it increases the number of solves in a given time, but it neither speeds up a single solve nor directly enables solving longer horizons. In contrast, temporal parallelization speeds up a single solve by representing the backward Riccati recursion and the forward rollout as associative scans, reducing their span from O(N)O(N) to O(log⁡N)O(\log N) [14], an approach used in GPU-accelerated iterative linear-quadratic regulator (iLQR) for legged robots [15].

The alternating direction method of multipliers (ADMM) is a distributed optimization algorithm that has become the backbone of several GPU-based convex solvers, but they are generic and do not exploit the full optimal control structure. Being relatively immature, they support fewer constraint types. For example, cuOSQP handles only quadratic programs [22], and cuClarabel restricts the dimension of semidefinite blocks [19]. The splitting conic solver (SCS) with GPU linear solver (cuDSS in backend) supports both second-order cone constraints and semidefinite constraints, but solves the generic Karush-Kuhn-Tucker (KKT) system and does not parallelize independent constraint projections [20].

ADMM has also been specialized for control under uncertainty on GPUs, for robust nonlinear model predictive control [17] and robust trajectory optimization [18]. As robust approaches assume bounded disturbances and account for worst-case uncertainty, they lead to overconservatism and infeasibility when the uncertainty is unbounded [23]. This literature gap is further explained in Table I.

WorkGau.CCLMICvxOCPlog NNPar.Bat.GPU
CS, exact [9, 10, 16]✓✓✓✓×××××
CS, approx. [7, 8]✓◐✓✓×✓✓××
Parallel LQR [14, 15]×××◐✓✓✓◐✓
Batched NLP [11, 12, 13]×××◐◐×◐✓✓
Robust ADMM [17], [18]×××◐◉◉✓✓✓
Conic solvers [19, 20, 21]−✓◐✓××◉×◉
socx (Ours)✓✓✓✓✓✓✓✓✓

Table I. Comparison with related work. CS: covariance steering. Problem class: Gaussian noise (Gau.), exact linear chance constraints (CC), covariance LMI (LMI), convex (Cvx). Solver structure: exploits OCP structure (OCP), time-parallel O(log⁡N)O(\log N) span (log NN), parallel within one problem (Par.), batches across problems (Bat.), GPU. ✓: yes, ◐: partial or only some works in the group, ×: no, −: not applicable.

Addressing these limitations, we present socx: GPU-accelerated chance-constrained stochastic optimal control using JAX. Our solver exploits the sparse and independent structure of the problem to reduce it to simpler problems: independent linear-quadratic tracking (LQT) problems and Euclidean projections onto sets. These simpler problems can be solved in parallel and then coupled using the constraints. accelerated algorithm implemented in JAX [24] that solves chance-constrained stochastic optimal control by exploiting the lifted convex formulation’s structure and achieves minimal conservatism [10]. The key contributions are: (i) a variable split that breaks the problem into independent LQT problems and closed-form parallel projection operations, (ii) an operator split of linear-quadratic tracking (LQT), P-RIC and P-LQT that enables sharing gains across stochastic channels, uses associative scans for efficient parallelization and is 2.5–4.0× faster per iteration than a sequential LQT, 2.2–7.5× faster than a condensed KKT (Table IV), (iii) a small projection operation for the terminal linear matrix inequality (LMI) constraint via an nx×nxn_x \times n_x Gram eigendecomposition instead of an SVD of the nx×ne(N)n_x \times n_e(N) factor or a PSD projection of size nx+ne(N)n_x + n_e(N), which is 22–216× faster per call than the Schur-complement PSD projection, and (iv) validation on 3D linearized quadrotor dynamics demonstrating speed-ups of 7–98× over MOSEK at 10−510^{-5} (N=40N = 40–240), 47–2224× faster than SCS+cuDSS. socx solves long-horizon up to N=1120N = 1120, whereas MOSEK runs out of memory at N=320N = 320 and SCS+cuDSS times out at N=640N = 640.

We first review the background on the lifted convex formulation of stochastic optimal control in § 2 and formulate parallel operators in Sec. III. Then, socx is presented in Sec. IV, with simulations and ablations in § 5 followed by the conclusion.

Notation: Boldface letters denote random variables (e.g., xkx_k, uku_k, ee, wkw_k). S++n\mathbb{S}^{n}_{++} (S+n\mathbb{S}^{n}_{+}) is the cone of n×nn \times n positive definite (semidefinite) matrices, InI_n is the identity, and ∥x∥A2=x⊤Ax\lVert x\rVert_A^2 = x^\top A x for A∈S+nA \in\mathbb{S}^{n}_{+}. Tr⁡(⋅)\operatorname{Tr}(\cdot), E[⋅]\mathbb{E}[\cdot], and var⁡(⋅)\operatorname{var}(\cdot) denote the trace, expectation, and variance. N(μ,Σ)\mathcal{N}(\mu,\Sigma) is a Gaussian with mean μ\mu and covariance Σ\Sigma, and ΦN(⋅)\Phi_{\mathcal{N}}(\cdot) and Φχn2(⋅)\Phi_{\chi^2_n}(\cdot) are the CDFs of the standard Gaussian and the chi-squared distribution with nn degrees of freedom.

Background: Stochastic Control

We consider the discrete-time stochastic linear time-varying system

xk+1=Akxk+Bkuk+Dkwk,x_{k+1} = A_k x_k + B_k u_k + D_k w_k,
x0∼N(μ0⋆,Σ0),wk∼N(0,Inw),(1)x_0 \sim\mathcal{N}(\mu_0^\star,\Sigma_0), \qquad w_k \sim\mathcal{N}(0,I_{n_w}), \tag*{(1)}

where xk∈Rnxx_k \in\mathbb{R}^{n_x}, uk∈Rnuu_k \in\mathbb{R}^{n_u}, and wk∈Rnww_k \in\mathbb{R}^{n_w} over a horizon NN.

Assumption 1. The initial state and process noises are mutually independent Gaussian random variables, E[wiwj⊤]=δijInw\mathbb{E}[w_iw_j^\top] = \delta_{ij}I_{n_w} and E[wix0⊤]=0\mathbb{E}[w_i x_0^\top] = 0. The control uku_k is causal and affine in x0,w0,…,wk−1x_0,w_0,\ldots,w_{k-1}; this policy class is denoted Πaffine\Pi_{\mathrm{affine}}.

Linear chance constraints on the state and control are

P(ai,k⊤xk≤bi,k)≥1−ϵx,i,k,P(αi,k⊤uk≤βi,k)≥1−ϵu,i,k,(2)\mathbb{P}(a_{i,k}^\top x_k \le b_{i,k}) \ge1-\epsilon_{x,i,k}, \qquad\mathbb{P}(\alpha_{i,k}^\top u_k \le\beta_{i,k}) \ge1-\epsilon_{u,i,k}, \tag*{(2)}

with ai,k∈Rnxa_{i,k} \in\mathbb{R}^{n_x}, αi,k∈Rnu\alpha_{i,k} \in\mathbb{R}^{n_u}, bi,k,βi,k∈Rb_{i,k},\beta_{i,k} \in\mathbb{R}, and ϵx,i,k,ϵu,i,k∈[0,0.5)\epsilon_{x,i,k},\epsilon_{u,i,k} \in[0,0.5). We additionally consider the terminal quadratic chance constraint

P(∥xN−E[xN]∥QCC2≤1)≥1−ϵN,(3)\mathbb{P}\left(\lVert x_N-\mathbb{E}[x_N]\rVert_{Q_{\mathrm{CC}}}^{2} \le1\right) \ge1-\epsilon_N, \tag*{(3)}

and the terminal covariance bound

var⁡(xN)⪯Σf,(4)\operatorname{var}(x_N) \preceq\Sigma_f, \tag*{(4)}

where QCC,Σf∈S++nxQ_{\mathrm{CC}},\Sigma_f \in\mathbb{S}^{n_x}_{++} and ϵN∈[0,1]\epsilon_N \in[0,1].

The finite-horizon stochastic optimal control problem is

Problem 1 (Linear Stochastic Optimal Control).

min⁡π∈ΠaffineJ(x,u)=∑t∈Twp∥E[xt]−μt⋆∥Qt2\min_{\pi\in\Pi_{\mathrm{affine}}} \mathcal{J}(x,u) = \sum_{t \in\mathcal{T}_{wp}} \lVert\mathbb{E}[x_t] - \mu_t^\star\rVert_{Q_t}^2
+E[∑k=0N−1(∥xk∥Qk2+∥uk∥Rk2)+∥xN−x⋆∥QN2],(5)+ \mathbb{E}\left[\sum_{k=0}^{N-1}\left(\lVert x_k\rVert_{Q_k}^2 + \lVert u_k\rVert_{R_k}^2\right) + \lVert x_N-x^\star\rVert_{Q_N}^2\right], \tag*{(5)}

subject to (1), (2), (3), and (4) for all k∈{0,1,…,N−1}k \in\{0,1,\ldots,N-1\}. Here Qk,QN,Qt⪰0Q_k,Q_N,Q_t \succeq0, Rk≻0R_k \succ0, x⋆∈Rnxx^\star\in\mathbb{R}^{n_x} is the terminal target, and μt⋆∈Rnx\mu_t^\star\in\mathbb{R}^{n_x} are waypoint targets for t∈Twp⊆{0,1,…,N}t \in\mathcal{T}_{wp} \subseteq\{0,1,\ldots,N\}.

Lifted Representation

Under Assumption 1, the state and control remain jointly Gaussian. Introduce the independent Gaussian basis

e(k+1):=[e(k)wk],e(0)∼N(0,Inx),(6)e^{(k+1)} := \begin{bmatrix} e^{(k)} \\ w_k \end{bmatrix}, \qquad e^{(0)} \sim\mathcal{N}(0,I_{n_x}), \tag*{(6)}

where x0=μ0⋆+Ψ0⋆e(0)x_0 = \mu_0^\star+ \Psi_0^\star e^{(0)}. Then e(k)∼N(0,Ine(k))e^{(k)} \sim\mathcal{N}(0,I_{n_e(k)}) with ne(k):=nx+knwn_e(k) := n_x + kn_w, and

xk=μk+Ψke(k),(7)x_k = \mu_k + \Psi_k e^{(k)}, \tag*{(7)}
uk=νk+Vke(k),(8)u_k = \nu_k + V_k e^{(k)}, \tag*{(8)}

with μk∈Rnx\mu_k \in\mathbb{R}^{n_x}, νk∈Rnu\nu_k \in\mathbb{R}^{n_u}, Ψk∈Rnx×ne(k)\Psi_k \in\mathbb{R}^{n_x \times n_e(k)}, and Vk∈Rnu×ne(k)V_k \in\mathbb{R}^{n_u \times n_e(k)}. Thus

E[xk]=μk,var⁡(xk)=ΨkΨk⊤,(9)\mathbb{E}[x_k] = \mu_k, \qquad\operatorname{var}(x_k) = \Psi_k\Psi_k^\top, \tag*{(9)}

with analogous expressions for uku_k.

The lifted dynamics are [10 Lemma 2]

μk+1=Akμk+Bkνk,(10)\mu_{k+1} = A_k\mu_k + B_k\nu_k, \tag*{(10)}
Ψk+1=[AkΨk+BkVkDk],(11)\Psi_{k+1} = \begin{bmatrix} A_k\Psi_k + B_kV_k & D_k \end{bmatrix}, \tag*{(11)}

with μ0=μ0⋆\mu_0 = \mu_0^\star and Ψ0=Ψ0⋆\Psi_0 = \Psi_0^\star.

For Gaussian variables, the linear chance constraints admit the exact second-order cone representation

ai,k⊤μk+ξx,i,k∥Ψk⊤ai,k∥2≤bi,k,(12)a_{i,k}^\top\mu_k + \xi_{x,i,k}\lVert\Psi_k^\top a_{i,k}\rVert_2 \le b_{i,k}, \tag*{(12)}
αi,k⊤νk+ξu,i,k∥Vk⊤αi,k∥2≤βi,k,(13)\alpha_{i,k}^\top\nu_k + \xi_{u,i,k}\lVert V_k^\top\alpha_{i,k}\rVert_2 \le\beta_{i,k}, \tag*{(13)}

where ξx,i,k=ΦN−1(1−ϵx,i,k)\xi_{x,i,k} = \Phi_{\mathcal{N}}^{-1}(1-\epsilon_{x,i,k}) and ξu,i,k=ΦN−1(1−ϵu,i,k)\xi_{u,i,k} = \Phi_{\mathcal{N}}^{-1}(1-\epsilon_{u,i,k}).

The terminal covariance bound is equivalent to

ΨNΨN⊤⪯Σf⟺[ΣfΨNΨN⊤Ine(N)]⪰0.(14)\Psi_N\Psi_N^\top\preceq\Sigma_f \quad\Longleftrightarrow\quad \begin{bmatrix} \Sigma_f & \Psi_N \\ \Psi_N^\top& I_{n_e(N)} \end{bmatrix} \succeq0. \tag*{(14)}

The quadratic chance constraint admits the conservative LMI approximation

\begin{bmatrix} \frac{1}{\zeta_Q}Q_{\mathrm{CC}} & \Psi_N \\ \Psi_N^\top& I_{n_e(N)} \end{bmatrix} \succeq0, \qquad \zeta_Q = \Phi_{\chi^2_{n_x}}^{-1}(1-\epsilon_N), \tag*{(15)}\end{bmatrix}

or the trace approximation

Tr⁡(ΨN⊤QCC−1ΨN)≤1(ΦN−1(1+nx1−ϵNnx2))2.(16)\operatorname{Tr}\left(\Psi_N^\top Q_{\mathrm{CC}}^{-1}\Psi_N\right) \le \frac{1}{\left(\Phi_{\mathcal{N}}^{-1}\left(\frac{1+n_x\sqrt[n_x]{1-\epsilon_N}}{2}\right)\right)^2}. \tag*{(16)}

Their relative conservatism is discussed in [10].

Convex Reformulation

Substituting the lifted representation gives the deterministic convex problem

Problem 2 (Lifted Convex Reformulation).

min⁡μ,ν,Ψ,V J=∑k=0N−1(∥μk∥Qk2+Tr⁡(QkΨkΨkT))+∑k=0N−1(∥νk∥Rk2+Tr⁡(RkVkVkT))+∥(μN−x⋆)∥QN2+Tr⁡(QNΨNΨNT)+∑i∈Twp∥(μi−μi⋆)∥Qi2(17)\begin{aligned} \min_{\mu,\nu,\Psi,V}\ J ={}& \sum_{k=0}^{N-1}\left(\|\mu_k\|_{Q_k}^2+\operatorname{Tr}(Q_k\Psi_k\Psi_k^{\mathsf T})\right)\\ &+\sum_{k=0}^{N-1}\left(\|\nu_k\|_{R_k}^2+\operatorname{Tr}(R_kV_kV_k^{\mathsf T})\right)\\ &+\|(\mu_N-x^\star)\|_{Q_N}^2+\operatorname{Tr}(Q_N\Psi_N\Psi_N^{\mathsf T})\\ &+\sum_{i\in T_{wp}}\|(\mu_i-\mu_i^\star)\|_{Q_i}^2 \tag*{(17)} \end{aligned}
s.t.(9),∀ k∈{0,1,…,N−1},(10),∀ i, ∀ k∈{0,1,…,N},(11),(12) or (13).\begin{aligned} \text{s.t.}\quad&(9),\quad\forall\ k\in\{0,1,\ldots,N-1\}, \\ &(10),\quad\forall\ i,\ \forall\ k\in\{0,1,\ldots,N\},\\ &(11),\quad(12)\ \text{or}\ (13). \end{aligned}

Theorem 1 (Exact Convex Reformulation). [10 Theorem 1] Under Assumption 1, Problem 2, restricted to (9), (10), and (11), is an exact deterministic convex reformulation of Problem 1: the two problems have equivalent costs and feasible affine policies.

Proposition 1 (Conservative Approximation of QCCs). [10 Proposition 1] Both (12) and (13) are sufficient approximations of (3); neither uniformly dominates the other.

Background: Parallel Primitives

In this section, we review parallel primitives that are packaged as an operator, P-LQT, for solving the Linear-Quadratic Tracking (LQT) problem using an associative-scan that evaluates in O(log⁡N)O(\log N) span that is invoked for mean and stochastic channels in Section IV.

Parallel Associative Scan

The associative property states that the grouping of operations does not matter, i.e., for an operator ∙\bullet, a∙(b∙c)=(a∙b)∙ca\bullet(b\bullet c)=(a\bullet b)\bullet c holds. Defining associative operations allows to compute (a1,a1∙a2,…,a1∙⋯∙an)(a_1,a_1\bullet a_2,\ldots,a_1\bullet\cdots\bullet a_n) from (a1,…,aN)(a_1,\ldots,a_N) in O(log⁡N)O(\log N) span as opposed to a sequential recursion in O(N)O(N) span [25].

To compute the rollouts of a dynamical system with specified feedback matrices, the following recursion is defined: yk+1=Mkyk+vky_{k+1}=M_ky_k+v_k, called an affine recurrence. This can be expressed as an associative operator, with the kk-th element being (Mk,vk)(M_k,v_k), and the operation being

(Mb,vb)∙(Ma,va)=(MbMa,Mbva+vb).(18)(M_b,v_b)\bullet(M_a,v_a)=(M_bM_a,M_bv_a+v_b). \tag*{(18)}

For prediction horizon NN, this enables the computation of rollouts in O(log⁡N)O(\log N) span, instead of O(N)O(N). The price is paid because it requires additional operations and memory to store the intermediate matrix [14, 15].

The Parallel LQT Operator

A finite-horizon LQT problem is defined as

min⁡ω ∑k=k0N(∥χk∥Q~k2−2q~kTχk)+∑k=k0N−1(∥ωk∥R~k2−2r~kTωk)\min_{\omega}\ \sum_{k=k_0}^{N}\left(\|\chi_k\|_{\widetilde{Q}_k}^2-2\widetilde{q}_k^{\mathsf T}\chi_k\right)+\sum_{k=k_0}^{N-1}\left(\|\omega_k\|_{\widetilde{R}_k}^2-2\widetilde{r}_k^{\mathsf T}\omega_k\right)
s.t.χk+1=Akχk+Bkωk,χk0 given,(19)\text{s.t.}\quad\chi_{k+1}=A_k\chi_k+B_k\omega_k,\qquad\chi_{k_0}\ \text{given}, \tag*{(19)}

with NN, k0k_0, χ\chi, ω\omega, AA, BB being the horizon, start step, state, control input, and transition matrices. And Q~⪰0\widetilde{Q}\succeq0, R~≻0\widetilde{R}\succ0 denote the quadratic costs for state and control with linear terms q~,r~\widetilde{q},\widetilde{r}. Therefore, a problem is fully specified by quadratic Q\mathcal{Q} and linear parameters L\mathcal{L},

Q:=(A0:N−1,B0:N−1,Q~0:N,R~0:N−1),\mathcal{Q}:=(A_{0:N-1},B_{0:N-1},\widetilde{Q}_{0:N},\widetilde{R}_{0:N-1}),
L:=(k0,χk0,q~k0:N,r~k0:N−1).(20)\mathcal{L}:=(k_0,\chi_{k_0},\widetilde{q}_{k_0:N},\widetilde{r}_{k_0:N-1}). \tag*{(20)}

here, the subscript i:ji:j denotes the set of matrices with the index varying from ii to jj. The optimal law can be represented as an affine-feedback law, ωk=Kkχk+kkff\omega_k=K_k\chi_k+k_k^{\mathrm{ff}}. Demonstrated in [14], ωk\omega_k can be computed from (Q,L)(\mathcal{Q},\mathcal{L}) in O(log⁡N)O(\log N) using an parallel associative scan. We will summarize only the properties and steps that we will use later.

We denote the value function at time step kk, the minimum cost of starting at state χ\chi at time kk, as

Vk(χ)=∥χ∥Pk2−2pkTχ+const.(21)\mathcal{V}_k(\chi)=\|\chi\|_{P_k}^2-2p_k^{\mathsf T}\chi+\mathrm{const}. \tag*{(21)}

Firstly, PkP_k can be obtained from Q\mathcal{Q} using associative scan [15] and then the feedback matrix, KkK_k can be constructed using the relation:

Kk=−Gk−1BkTPk+1Ak,Gk:=R~k+BkTPk+1Bk.(22)K_k=-G_k^{-1}B_k^{\mathsf T}P_{k+1}A_k,\qquad G_k:=\widetilde{R}_k+B_k^{\mathsf T}P_{k+1}B_k. \tag*{(22)}

We package this as an operator, P-RIC, which has O(log⁡N)O(\log N) span, O(N(nx3+nu3))O(N(n_x^3+n_u^3)) work and O(Nnx2)O(Nn_x^2) memory,

(P0:N,K0:N−1)=P-RIC(Q).(23)(P_{0:N},K_{0:N-1})=\mathrm{P\text{-}RIC}(\mathcal{Q}). \tag*{(23)}

Additionally, following Bellman optimality, (Pk,Kk)(P_k,K_k) depend only on (A,B,Q~,R~)k:N(A,B,\widetilde{Q},\widetilde{R})_{k:N}, so an instance that starts at k0>0k_0>0 can use the same solve.

Using the feedback matrix, the linear term of the value function, pkp_k, can be computed using L\mathcal{L} as the recursion

pk=(Ak+BkKk)Tpk+1+q~k+KkTr~k,(24)p_k=(A_k+B_kK_k)^{\mathsf T}p_{k+1}+\widetilde{q}_k+K_k^{\mathsf T}\widetilde{r}_k, \tag*{(24)}

which is an affine recurrence relation in pkp_k, with pN=q~Np_N=\widetilde{q}_N. This can be obtained in O(log⁡N)O(\log N) as defined in Section III-A. The feedforward input kkffk_k^{\mathrm{ff}} can be computed as

kkff=Gk−1(r~k+BkTpk+1),(25)k_k^{\mathrm{ff}}=G_k^{-1}(\widetilde{r}_k+B_k^{\mathsf T}p_{k+1}), \tag*{(25)}

Now, as the control input is ωk=Kkχk+kkff\omega_k=K_k\chi_k+k_k^{\mathrm{ff}} and the optimal states can be defined using the affine recurrence relation, as the initial state is given:

χk+1=(Ak+BkKk)χk+Bkkkff.(26)\chi_{k+1}=(A_k+B_kK_k)\chi_k+B_kk_k^{\mathrm{ff}}. \tag*{(26)}

Combining the two affine recurrences (21) and (23), we package it as an operator:

(χk0:N,ωk0:N−1)=P-LQT(Q,P,K,L).(27)(\chi_{k_0:N},\omega_{k_0:N-1})=\mathrm{P\text{-}LQT}(\mathcal{Q},P,K,\mathcal{L}). \tag*{(27)}

For fixed (Q,P,K)(Q,P,K), the map L↦(χ,ω)\mathcal{L} \mapsto(\chi,\omega) given by (21), (22), (23) is linear in (χk0,q~,r~)(\chi_{k0},\tilde{q},\tilde{r}). If it is invoked WW times, in parallel, with shared (Q,P,K)(Q,P,K) then it has O(log⁡N)O(\log N) span, O(Nnx2+WNnx2)O(Nn_x^2+WNn_x^2) work and O(WNnx)O(WNn_x) memory.

Method

In this section, we present socx by describing (i) the structure of Problem 2; (ii) ADMM with the variable split; (iii) the θ\theta-update using P-RIC/P-LQT operations; and (iv) the zz-update as closed-form projections for second-order cone and covariance constraint, as summarized in Figure 1. The resulting solver, socx, does not require Jacobians, KKT matrices, or dense constraint operators.

Steps of ADMM

Figure 1. Steps of ADMM (28) in socx. In one iteration, the θ\theta-update solves mean and stochastic columns’ LQT problems in parallel, followed by the zz-update, which projects every chance-constraint cone and the terminal covariance ball in parallel.

Structure of the Lifted Problem

The number of variables in Problem 2 grows as O(N2)O(N^2) because of the new elements in the stochastic basis. However, the problem is highly structured.

Shared dynamics. Let ςk(c):=Ψk[:,c]\varsigma_k^{(c)}:=\Psi_k[:,c] and υk(c):=Vk[:,c]\upsilon_k^{(c)}:=V_k[:,c] denote the columns of Ψ\Psi and VV. The following equations govern the stochastic columns (ς(c),υ(c))(\varsigma^{(c)},\upsilon^{(c)}), obtained from (9b):

ςk+1(c)=Akςk(c)+Bkυk(c),k≥kc,ςk(c)=0,υk(c)=0,k<kc.(28)\begin{aligned} \varsigma_{k+1}^{(c)}&=A_k\varsigma_k^{(c)}+B_k\upsilon_k^{(c)}, && k\geq k_c,\\ \varsigma_k^{(c)}&=0,\quad\upsilon_k^{(c)}=0, && k<k_c. \tag*{(28)} \end{aligned}

The initial conditions are defined as:

(kc,ς0(c))={(0,Ψ0⋆[:,c]),c<nx,(⌊c−nxnw⌋+1,Dkc−1[:,rc]),c≥nx,(29)(k_c,\varsigma_0^{(c)})= \begin{cases} (0,\Psi^\star_{0}[:,c]), & c<n_x,\\ \left(\left\lfloor\frac{c-n_x}{n_w}\right\rfloor+1,D_{k_c-1}[:,r_c]\right), & c\geq n_x, \end{cases} \tag*{(29)}

with rc:=(c−nx) mod nwr_c:=(c-n_x)\bmod n_w. Similarly, the mean dynamics are independent of stochastic columns (9a).

Cost separation. In the lifted cost function (14a), the stochastic columns appear through the same matrices, QkQ_k and RkR_k, and there are no coupling terms across columns.

Tr⁡(QkΨkΨk⊤)=∑c∥ςk(c)∥Qk2,\operatorname{Tr}(Q_k\Psi_k\Psi_k^\top)=\sum_c\lVert\varsigma_k^{(c)}\rVert_{Q_k}^2,

and likewise for RkR_k and VkV_k. The mean variables aren’t coupled with the stochastic columns in the lifted cost function.

Coupling constraints. Second-order cone constraints couple the mean and stochastic channels (14c). The quadratic and the linear matrix inequality constraint couples the stochastic columns (14d).

Dynamics and cost are separable and coupled only through the constraints; therefore, this motivates an optimization algorithm that allows solving subproblems and pooling using the constraints. This structure is what ADMM is designed to exploit when an appropriate variable split is used.

ADMM & Variable Splitting

We review ADMM, a popular algorithm for distributed convex optimization, and then define a variable split. ADMM is well-suited to large-scale problems and combines decomposability with superior convergence properties [26]. It solves problems of the form

min⁡θ,zf(θ)+g(z)(30)\min_{\theta,z}\quad f(\theta)+g(z) \tag*{(30)}
s.t.Mθ+Ez=c(31)\text{s.t.}\quad\mathcal{M}\theta+\mathcal{E}z=c \tag*{(31)}

by formulating the augmented Lagrangian

Lρ(θ,z,y)=f(θ)+g(z)+y⊤(Mθ+Ez−c)+ρ2∥Mθ+Ez−c∥22,L_\rho(\theta,z,y)=f(\theta)+g(z)+y^\top(\mathcal{M}\theta+\mathcal{E}z-c)+\frac{\rho}{2}\lVert\mathcal{M}\theta+\mathcal{E}z-c\rVert_2^2,

and iterating over the update steps

θ-update: θj+1:=arg⁡min⁡θ Lρ(θ,zj,yj)(32)\theta\text{-update}:\ \theta^{j+1}:=\underset{\theta}{\arg\min}\ L_\rho(\theta,z^j,y^j) \tag*{(32)}
z-update: zj+1:=arg⁡min⁡z Lρ(θj+1,z,yj)(33)z\text{-update}:\ z^{j+1}:=\underset{z}{\arg\min}\ L_\rho(\theta^{j+1},z,y^j) \tag*{(33)}
y-update: yj+1:=yj+ρ(Mθj+1+Ezj+1−c)(34)y\text{-update}:\ y^{j+1}:=y^j+\rho(\mathcal{M}\theta^{j+1}+\mathcal{E}z^{j+1}-c) \tag*{(34)}

with ρ>0\rho>0.

Introducing the scaled dual variable u:=y/ρu:=y/\rho and completing the squares, the last two terms of the augmented Lagrangian, for the case E=−I\mathcal{E}=-I, become

Lρ(θ,z,ρu)=f(θ)+g(z)+ρ2∥Mθ−z+u∥22−ρ2∥u∥22(35)L_\rho(\theta,z,\rho u)=f(\theta)+g(z)+\frac{\rho}{2}\lVert\mathcal{M}\theta-z+u\rVert_2^2-\frac{\rho}{2}\lVert u\rVert_2^2 \tag*{(35)}

This squared penalty term turns the θ\theta-update into an LQT (16).

Variable splitting. Collect the primal variables as

θ:=(μ0:N,ν0:N−1,Ψ0:N,V0:N−1).\theta:=(\mu_{0:N},\nu_{0:N-1},\Psi_{0:N},V_{0:N-1}).

For each cone constraint, we introduce a split variable whose size matches the cone. Let zx,i,k,zu,i,k∈R1+ne(k)z_{x,i,k},z_{u,i,k}\in\mathbb{R}^{1+n_e(k)} correspond to the state and control cone constraint, defined as

zx,i,k=Mx,i,kθ:=(bi,k−ai,k⊤μk,ςi,k⊤Ψk⊤ai,k)(36)z_{x,i,k}=\mathcal{M}_{x,i,k}\theta:=(b_{i,k}-a_{i,k}^{\top}\mu_k,\varsigma_{i,k}^{\top}\Psi_k^{\top}a_{i,k}) \tag*{(36)}
zu,i,k=Mu,i,kθ:=(βi,k−αi,k⊤νk,ςu,i,k⊤Vk⊤αi,k).z_{u,i,k}=\mathcal{M}_{u,i,k}\theta:=(\beta_{i,k}-\alpha_{i,k}^{\top}\nu_k,\varsigma_{u,i,k}^{\top}V_k^{\top}\alpha_{i,k}).

Additionally, depending on the terminal constraint, either (11) or (12), the constraint takes the form ΨNΨN⊤⪯Σˉ\Psi_N\Psi_N^\top\preceq\bar{\Sigma}, with Σˉ=Σf\bar{\Sigma}=\Sigma_f or Σˉ=ΣQ⊗ΣQC\bar{\Sigma}=\Sigma_Q\otimes\Sigma_{QC} respectively.

Remark 1. The discussion is limited to the linear matrix inequality as the trace relaxation (13) appears as a quadratic constraint that can be expressed as second-order cone constraint.

Hence, the corresponding split variable zN∈Rnx×ne(N)z_N\in\mathbb{R}^{n_x\times n_e(N)} is defined as zN=MNθ:=Σˉ−1/2ΨNz_N=\mathcal{M}_N\theta:=\bar{\Sigma}^{-1/2}\Psi_N. Collecting the split variables, z:={zx,i,k,zu,i,k,zN}z:=\{z_{x,i,k},z_{u,i,k},z_N\}, an affine operator can be defined as Mθ=z\mathcal{M}\theta=z.

Splitting methods handle a convex constraint set C\mathcal{C} by moving it out of the smooth objective and into the indicator function

IC(z)={0,z∈C+∞,z∉C,(37)\mathbb{I}_{\mathcal{C}}(z)= \begin{cases} 0, & z\in\mathcal{C}\\ +\infty, & z\notin\mathcal{C}, \end{cases} \tag*{(37)}

which is convex and closed whenever C\mathcal{C} is. Hence, resulting in the following ADMM split of Problem 2:

f(θ)=J(θ)+IS(θ),f(\theta)=J(\theta)+\mathbb{I}_{\mathcal{S}}(\theta),
g(z)=∑i,kIKne(k)(zx,i,k)+∑i,kIKne(k)(zu,i,k)+IB(zN),g(z)=\sum_{i,k}\mathbb{I}_{\mathcal{K}_{n_e(k)}}(z_{x,i,k})+\sum_{i,k}\mathbb{I}_{\mathcal{K}_{n_e(k)}}(z_{u,i,k})+\mathbb{I}_{\mathcal{B}}(z_N),
s.t.Mθ−z=0(38)\text{s.t.}\quad\mathcal{M}\theta-z=0 \tag*{(38)}

where Kn={(t,s)∈R×Rn:∥s∥2≤t}\mathcal{K}_n=\{(t,s)\in\mathbb{R}\times\mathbb{R}^n:\lVert s\rVert_2\leq t\} for the second-order cone constraints and B={Z∈Rnx×ne(N):∥Z∥2≤1}\mathcal{B}=\{Z\in\mathbb{R}^{n_x\times n_e(N)}:\lVert Z\rVert_2\leq1\} for the terminal covariance constraint. Under this split, the θ\theta-update becomes independent LQT solves and the zz-update becomes closed-form set projections as described in the next subsections.

Convergence monitoring. The primal (rpjr_p^j) and dual residuals (rdjr_d^j) are defined as

rpj=∥Mθj+Ezj−c∥2,rdj=ρ∥MTE(zj−zj−1)∥2.(39)r_p^j = \lVert M\theta^j + Ez^j-c\rVert_2,\quad r_d^j = \rho\lVert M^\mathsf{T}E(z^j-z^{j-1})\rVert_2. \tag*{(39)}

With an initialization of z0=0z^0=0 and u0=0u^0=0, socx monitors the following two conditions and stops when both hold

∥Mθj−zj∥∞≤ϵabs+ϵrelmax⁡(∥Mθj∥∞,∥zj∥∞),\lVert M\theta^j-z^j\rVert_\infty\leq\epsilon_{\mathrm{abs}}+\epsilon_{\mathrm{rel}}\max\left(\lVert M\theta^j\rVert_\infty,\lVert z^j\rVert_\infty\right),
ρ∥zj−zj−1∥∞≤ϵabs+ϵrelρ∥uj∥∞,(40)\rho\lVert z^j-z^{j-1}\rVert_\infty\leq\epsilon_{\mathrm{abs}}+\epsilon_{\mathrm{rel}}\rho\lVert u^j\rVert_\infty, \tag*{(40)}

where ϵrel\epsilon_{\mathrm{rel}} and ϵabs\epsilon_{\mathrm{abs}} are the relative and absolute tolerance parameters [22].

The value of ρ\rho determines the convergence rate, and adaptive algorithms help reduce the number of iterations by scaling ρ\rho based on the residuals [17], [26]

ρ+←ρ⋅clip⁡(rpjrdj,κ‾,κ‾),u+←uρρ+.(41)\rho^+ \leftarrow\rho\cdot\operatorname{clip}\left(\sqrt{\frac{r_p^j}{r_d^j}},\underline{\kappa},\overline{\kappa}\right),\qquad u^+ \leftarrow\frac{u\rho}{\rho^+}. \tag*{(41)}

thereby eliminating the need to hand-tune ρ\rho and driving the residuals to a comparable magnitude, with the safeguards κ‾=1/2\underline{\kappa}=1/2, κ‾=2\overline{\kappa}=2.

Remark 2 (Over-relaxation). A standard empirical accelerator is over-relaxation, which substitutes Mθj+1M\theta^{j+1} with αorMθj+1+(1−αor)(c−Ezj)\alpha_{\mathrm{or}}M\theta^{j+1}+(1-\alpha_{\mathrm{or}})(c-Ez^j) in the zz- and uu-updates. Convergence is retained for αor∈(0,2)\alpha_{\mathrm{or}}\in(0,2), and αor∈[1.5,1.8]\alpha_{\mathrm{or}}\in[1.5,1.8] improves the linear rate at no per-iteration cost [26].

θ\theta-update as Independent LQT Invocations

Under the split (32), the θ\theta-update step (28a) keeps the lifted dynamics (9) as equality constraint,

θj+1=arg⁡min⁡θ J(θ)+ρ2∥Mθ−τ∥22\theta^{j+1}=\underset{\theta}{\arg\min}\ J(\theta)+\frac{\rho}{2}\lVert M\theta-\tau\rVert_2^2
s.t. (9)(42)\text{s.t. }(9) \tag*{(42)}

here, τ:=zj−uj\tau:=z^j-u^j. For all the columns, the added penalty term is a sum of squares of components of MθM\theta that acts on (μ,ν,Ψ,V)(\mu,\nu,\Psi,V) column-wise, without any cross-column coupling. Hence, θ\theta-update (36) splits into 1+ne(N)1+n_e(N) independent instances of LQT solves (16), consisting of one mean channel (χ,ω)↦(μ,ν)(\chi,\omega)\mapsto(\mu,\nu) and ne(N)n_e(N) stochastic channels (χ,ω)↦(ζ(c),υ(c))(\chi,\omega)\mapsto(\zeta^{(c)},\upsilon^{(c)}).

Hence, the defined operators can be used to solve this step with the arguments (Q,L)(Q,\mathcal{L}) described in this subsection and summarized in Table II.

mean channelstochastic channel cc
(χk,ωk)(\chi_k,\omega_k)(μk,νk)(\mu_k,\nu_k)(ςk(c),νk(c))(\varsigma_k^{(c)},\nu_k^{(c)})
Q:(Q~k,R~k)\mathcal{Q}: (\widetilde{Q}_k,\widetilde{R}_k)(Q~kμ,R~kμ)(\widetilde{Q}^{\mu}_k,\widetilde{R}^{\mu}_k)(Q~kς,R~kς)(\widetilde{Q}^{\varsigma}_k,\widetilde{R}^{\varsigma}_k)
L:k0\mathcal{L}: k_000kck_c (26)
L:χk0\mathcal{L}: \chi_{k_0}μ0⋆\mu_0^\starςkc(c)\varsigma_{k_c}^{(c)} (26)
L:(q~k,r~k)\mathcal{L}: (\widetilde{q}_k,\widetilde{r}_k)(q~kμ,r~kμ)(\widetilde{q}^{\mu}_k,\widetilde{r}^{\mu}_k)(q~kς,(c),r~kς,(c))(\widetilde{q}^{\varsigma,(c)}_k,\widetilde{r}^{\varsigma,(c)}_k)
count11ne(N)n_e(N)
P-RIC calls / ρ\rho update1111, shared
P-LQT calls / iteration11ne(N)n_e(N), batched

Table II. P-RIC and P-LQT arguments with shared (Ak,Bk)(A_k,B_k).

Quadratic data. Collecting the base cost and the ρ\rho-penalty gives the augmented stage weights

Q~kμ:=Qk+ρ2∑iai,kai,kT+1[k∈Twp]Q⋆,(43)\widetilde{Q}_k^\mu:=Q_k+\frac{\rho}{2}\sum_i a_{i,k}a_{i,k}^{\mathsf T}+\mathbf{1}_{[k\in T_{\mathrm{wp}}]}Q_\star, \tag*{(43)}
R~kμ:=Rk+ρ2∑iαi,kαi,kT,(44)\widetilde{R}_k^\mu:=R_k+\frac{\rho}{2}\sum_i\alpha_{i,k}\alpha_{i,k}^{\mathsf T}, \tag*{(44)}
Q~kς:=Qk+ρ2∑iξx,i,kξx,i,kT,(45)\widetilde{Q}_k^\varsigma:=Q_k+\frac{\rho}{2}\sum_i \xi_{x,i,k}\xi_{x,i,k}^{\mathsf T}, \tag*{(45)}
R~kς:=Rk+ρ2∑iξu,i,kξu,i,kT,(46)\widetilde{R}_k^\varsigma:=R_k+\frac{\rho}{2}\sum_i \xi_{u,i,k}\xi_{u,i,k}^{\mathsf T}, \tag*{(46)}
Q~Nμ:=QN+ρ2∑iai,Nai,NT+1[N∈Twp]Q⋆,(47)\widetilde{Q}_N^\mu:=Q_N+\frac{\rho}{2}\sum_i a_{i,N}a_{i,N}^{\mathsf T}+\mathbf{1}_{[N\in T_{\mathrm{wp}}]}Q_\star, \tag*{(47)}
Q~Nς:=QN+ρ2∑iξx,i,Nξx,i,NT+ρ2Σ−1.(48)\widetilde{Q}_N^\varsigma:=Q_N+\frac{\rho}{2}\sum_i \xi_{x,i,N}\xi_{x,i,N}^{\mathsf T}+\frac{\rho}{2}\Sigma^{-1}. \tag*{(48)}

Here, 1\mathbf{1} is an indicator function which is 1 if k∈Twpk\in T_{\mathrm{wp}}, otherwise 0. And (Q~μ,Q~ς)(R~μ,R~ς)(\widetilde{Q}^\mu,\widetilde{Q}^\varsigma)(\widetilde{R}^\mu,\widetilde{R}^\varsigma) are the quadratic costs for the state (control) in the mean and stochastic channels. They differ in the quadratic costs because of the different way the chance constraints show up and due to the waypoint costs on the mean. Additionally, the terminal covariance constraint only shows up for the stochastic channels.

Linear data. The ADMM target τ\tau enters only through linear terms, and differs across each of the stochastic channel. Let the scalar and vector parts of the cone (i,k)(i,k) be denoted by (τi,kt,τi,ks)(\tau^t_{i,k},\tau^s_{i,k}), and let TT be the target of the terminal ball. To obtain the linear terms, q~,r~\widetilde{q},\widetilde{r}, the penalty in (36) together with the two reference-tracking terms of (14a) is expanded:

q~kμ:=ρ2∑i(bi,k−τx,i,kt)ai,k+δkNQNx⋆+1[k∈Twp]Q⋆μk⋆,(49)\widetilde{q}_k^\mu:=\frac{\rho}{2}\sum_i(b_{i,k}-\tau^t_{x,i,k})a_{i,k}+\delta_{kN}Q_Nx^\star+\mathbf{1}_{[k\in T_{\mathrm{wp}}]}Q_\star\mu_k^\star, \tag*{(49)}
r~kμ:=ρ2∑i(βi,k−τu,i,kt)αi,k,(50)\widetilde{r}_k^\mu:=\frac{\rho}{2}\sum_i(\beta_{i,k}-\tau^t_{u,i,k})\alpha_{i,k}, \tag*{(50)}
q~ks,(c):=ρ2∑iξx,i,kτx,i,k,csai,k+δkNρ2Σ−1/2T:,c,(51)\widetilde{q}_k^{s,(c)}:=\frac{\rho}{2}\sum_i\xi_{x,i,k}\tau^s_{x,i,k,c}a_{i,k}+\delta_{kN}\frac{\rho}{2}\Sigma^{-1/2}T_{:,c}, \tag*{(51)}
r~ks,(c):=ρ2∑iξu,i,kτu,i,k,csαi,k,k≥kc.(52)\widetilde{r}_k^{s,(c)}:=\frac{\rho}{2}\sum_i\xi_{u,i,k}\tau^s_{u,i,k,c}\alpha_{i,k},\qquad k\geq k_c. \tag*{(52)}

Dynamics data. The dynamics equations are shared as described for the stochastic channels in (25) with different initial conditions (26), and for the mean channels as defined in (9a).

Invocation. The first step in θ\theta-update is evaluated as parallel Riccati scans:

(Pμ,Kμ)←P-RIC⁡(Qμ),(53)(P^\mu,K^\mu) \leftarrow\operatorname{P\text{-}RIC}(Q^\mu), \tag*{(53)}
(Pς,Kς)←P-RIC⁡(Qς),(54)(P^\varsigma,K^\varsigma) \leftarrow\operatorname{P\text{-}RIC}(Q^\varsigma), \tag*{(54)}

followed by affine recurrences scans that use the same feedback gains for the stochastic channels:

(μ,ν)←P-LQT⁡(Qμ,Pμ,Kμ,Lμ),(55)(\mu,\nu) \leftarrow\operatorname{P\text{-}LQT}(Q^\mu,P^\mu,K^\mu,\mathcal{L}^\mu), \tag*{(55)}
(ς(c),ν(c))←P-LQT⁡(Qς,Pς,Kς,L(c)),(56)(\varsigma^{(c)},\nu^{(c)}) \leftarrow\operatorname{P\text{-}LQT}(Q^\varsigma,P^\varsigma,K^\varsigma,\mathcal{L}^{(c)}), \tag*{(56)}

c∈{0,1,…,ne(N)−1}c \in\{0,1,\ldots,n_e(N)-1\}, where (40) is evaluated only when ρ\rho changes, and

Qμ:=(A,B,Q~μ,R~μ),Qς:=(A,B,Q~ς,R~ς),\mathcal{Q}^\mu:= (A,B,\widetilde{Q}^\mu,\widetilde{R}^\mu), \qquad\mathcal{Q}^\varsigma:= (A,B,\widetilde{Q}^\varsigma,\widetilde{R}^\varsigma),
Lμ:=(0,μ0⋆,q~0:Nμ,r~0:N−1μ),\mathcal{L}^\mu:= (0,\mu_0^\star,\widetilde{q}_{0:N}^\mu,\widetilde{r}_{0:N-1}^\mu),
L(c):=(kc,ςkc(c),q~kc:Nς,(c),r~kc:N−1ς,(c)).(57)\mathcal{L}^{(c)} := (k_c,\varsigma_{k_c}^{(c)},\widetilde{q}_{k_c:N}^{\varsigma,(c)},\widetilde{r}_{k_c:N-1}^{\varsigma,(c)}). \tag*{(57)}

Hence, SOCX solves LQT many times with shared gains, rather than formulating a single large conic program.

zz-update as Parallel Projections

In the minimization problem of zz-update, when the cost gg (38) is a sum of indicators, the update reduces to the proximal operator of the individual indicator, i.e. the Euclidean projection,

zj+1=arg⁡min⁡z∈C 12∥z−(Mθj+1+uj)∥2z^{j+1}=\underset{z\in\mathcal{C}}{\arg\min}\ \frac{1}{2}\lVert z-(\mathcal{M}\theta^{j+1}+u^j)\rVert^2
:=ΠC(Mθj+1+uj).(58):=\Pi_{\mathcal{C}}(\mathcal{M}\theta^{j+1}+u^j). \tag*{(58)}

The projections are executed in parallel because the sets are independent; i.e., no variable is projected in both sets simultaneously.

Second-order cone constraints. For the projection to the set Kn\mathcal{K}_n, we obtain

ΠKn(t,s)={(t,s),∥s∥2≤t,(0,0),∥s∥2≤−t,t+∥s∥22(1,s∥s∥2),otherwise.(59)\Pi_{\mathcal{K}_n}(t,s)= \begin{cases} (t,s), & \lVert s\rVert_2\le t,\\ (0,0), & \lVert s\rVert_2\le-t,\\ \frac{t+\lVert s\rVert_2}{2}\left(1,\frac{s}{\lVert s\rVert_2}\right), & \text{otherwise}. \end{cases} \tag*{(59)}

Therefore, each of zx,i,kz_{x,i,k}, similarly for zu,i,kz_{u,i,k}, is updated as:

zx,i,kj+1=ΠKne(k)(Mx,i,kθj+1+ux,i,kj).(60)z_{x,i,k}^{j+1}=\Pi_{\mathcal{K}_{n_e(k)}}(\mathcal{M}_{x,i,k}\theta^{j+1}+u_{x,i,k}^{j}). \tag*{(60)}

here, Mx,i,k\mathcal{M}_{x,i,k} is as defined in (36) and ux,i,ku_{x,i,k} is the corresponding component of uu.

Terminal covariance constraint. For projection of X∈Rnx×ne(N)X\in\mathbb{R}^{n_x\times n_e(N)} to the set B\mathcal{B}, the standard approach is computing the singular value decomposition of X=UXΛVXTX=U_X\Lambda V_X^\mathsf{T} and then ΠB(X)=UXmin⁡(Λ,Inx)VXT\Pi_{\mathcal{B}}(X)=U_X\min(\Lambda,I_{n_x})V_X^\mathsf{T} with min⁡\min denoting an element-wise minimum of the two diagonal matrices. However, as XX is a fat matrix and we don’t require the matrix VXV_X, we can instead compute the eigendecomposition of XXT=UXΛX2UXTXX^\mathsf{T}=U_X\Lambda_X^2U_X^\mathsf{T}, giving us UXU_X and ΛX2\Lambda_X^2, from which we construct MX:=UXmin⁡(Inx,ΛX−1)UXTM_X:=U_X\min(I_{n_x},\Lambda_X^{-1})U_X^\mathsf{T}. Therefore, the projection operator becomes:

ΠB(X)=UXmin⁡(Λ,Inx)VXT=UXmin⁡(Inx,ΛX−1)(UXTUX)ΛXVXT=MXX.(61)\begin{aligned} \Pi_{\mathcal{B}}(X)=U_X\min(\Lambda,I_{n_x})V_X^\mathsf{T} \\ &=U_X\min(I_{n_x},\Lambda_X^{-1})(U_X^\mathsf{T}U_X)\Lambda_XV_X^\mathsf{T}=M_XX. \tag*{(61)} \end{aligned}

Hence, the zNz_N update is

zNj+1=ΠB(MNθj+1+uNj)(62)z_N^{j+1}=\Pi_{\mathcal{B}}(\mathcal{M}_N\theta^{j+1}+u_N^j) \tag*{(62)}

Hence, the zz-update reduces to closed-form Euclidean projections onto the cone and covariance constraint considered, which can be evaluated in a parallel, batched process.

Numerical Simulations

In this section, we present numerical simulations with ablations, run on a system with an Intel Core Ultra 9 285K (64GB RAM) and an RTX 5080 (16GB VRAM). To demonstrate the efficacy of SOCX, we consider a hover-linearized quadrotor with state (p,v,ϕ,θ,ψ,ω)∈R12(p,v,\phi,\theta,\psi,\omega)\in\mathbb{R}^{12}, inputs (ΔT,τx,τy,τz)∈R4(\Delta T,\tau_x,\tau_y,\tau_z)\in\mathbb{R}^4, and noise w∼N(0,I6)w\sim\mathcal{N}(0,I_6) entering as forces and torques, with Euler discretization and Δt=0.1s\Delta t=0.1\mathrm{s}. We consider three different setups: a helix in a tapering corridor (13 waypoints), a zig-zag climb, and an out-and-back (4 waypoints each), shown in Figure 2. At N=40N=40, we reduce the waypoints to 3, 2, and 2, respectively, as the 4-s horizon is too short to fly through all of them.

3D quadrotor trajectories computed with socx, with random wrench perturbations, chance constraints on position and actuator limits and terminal covariance constraint

Figure 2. 3D quadrotor trajectories computed with socx, with random wrench perturbations, chance constraints on position & actuator limits and terminal covariance constraint (nx=12n_x = 12, nw=6n_w = 6, nu=4n_u = 4, N=80N = 80, Δt=0.1\Delta t = 0.1).

The walls are represented as six half-plane linear chance constraints (2) in position ϵx=0.05\epsilon_x=0.05, and actuator limits are imposed as both an upper and lower limit with ϵu=0.05\epsilon_u=0.05 at each time step. The terminal state has a position-covariance constraint (4) with Σf=(0.12 m)2I3\Sigma_f=(0.12\,\mathrm{m})^2I_3. A selector matrix of appropriate size can be introduced to impose such a terminal covariance constraint. A quadratic penalty on state and control input is imposed. The waypoint and goal terms act only on the mean state.

SOCX vs. MOSEK vs. SCS+cuDSS: Tolerance vs. Solve Time

For baseline comparisons, we report the internal solve times of MOSEK [21] and SCS (cuDSS), both called through CVXPY [27]. SOCX’s measured time excludes JAX’s just-in-time (JIT) compilation [24]. Figure 3 and Table III compare the solve times. Compared to MOSEK, the speed-up grows from 7.2–7.9× at N=40N=40 to 57–98× at $N=240 because MOSEK’s time grows faster than SOCX. MOSEK runs out of memory at N=320N=320, while SOCX solves N=1120N=1120 in 145 s on average. As MOSEK is an interior-point method, its solve time does not vary much with tolerance, unlike socx which is a first-order method. We observe that socx remains faster at tight tolerances, but by a smaller margin. At N=160N = 160 the speed-up is 37–53× at 10−510^{-5} and still 12–14× at 10−710^{-7}. SCS (cuDSS), a GPU-based first-order method, is 47–2224× slower than socx and slower than MOSEK for N≥80N \ge80. We additionally observe that SCS times out at a 10,000s time cap for N≥640N \ge640. In Table III, PviolP_{\mathrm{viol}} denotes the probability of violation divided by the allowed violation. A value close to 1 indicates that the constraints are active, and a value greater than 1 quantifies the violation.

Figure 3. Solve time vs. (a) tolerance at fixed N=160N = 160 and (b) horizon at fixed ϵ=10−5\epsilon= 10^{-5}

θ\theta-update: LQT ablations

Table IV compares the per-iteration cost of the parallel LQT update with three alternatives. Sequential Riccati updates have O(N)O(N) span and use 2.5–4.9× less memory, but are 2.5–4.0× slower. The condensed KKT update uses 3.9–9.5× less memory but is 2.2–7.5× slower, and its time grows 3.9× and 6.4× per doubling of NN. The key comparison is with a per-column loop that does not exploit the fact that the stochastic columns share QQ and therefore (P,K)(P, K), so the gains cannot be reused (39b). It is 6.9–14.8× slower and uses 4.8–9.7× more memory than the shared-gain update, and both gaps widen as NN grows.

zz-update: Covariance ablations

We consider three different ways to project onto the terminal covariance constraint (11). A generic conic solver must lift it to an LMI constraint of size (nx+W)×(nx+W)(n_x+W) \times(n_x+W) as follows

∥X∥2≤1⟺[InxXXTIne(N)]∈Snx+ne(N)+,(63)\lVert X \rVert_2 \le1 \Longleftrightarrow \begin{bmatrix} I_{n_x} & X \\ X^{\mathsf{T}} & I_{n_e(N)} \end{bmatrix} \in\mathbb{S}^{+}_{n_x+n_e(N)}, \tag*{(63)}

with W:=ne(N)W := n_e(N) and project onto Snx+W+\mathbb{S}^{+}_{n_x+W} with an eigendecomposition of cost O((nx+W)3)O((n_x + W)^3). However, socx works with spectral-norm ball projections using the nx×nxn_x \times n_x matrix. We also compare against it by computing the SVD of the full matrix XX, which has the same asymptotic complexity but a larger constant. In Table IV, the Gram eigendecomposition takes 0.39–0.42 ms per call, nearly independent of NN over this range; the SVD is 2.4–2.5× slower, and the Schur-complement PSD projection is 22–216× slower, with the gap growing with NN.

Remark 3. Batching can be used to increase solve throughput at the expense of added latency by solving multiple instances in parallel, which often increases the individual solve time [11, 12, 13].

Conclusion

We presented socx, a GPU-accelerated JAX implementation for solving linear stochastic optimal control with exact representations for linear chance constraints and terminal covariance constraints, with approximate representations for quadratic chance constraints. The proposed solver exploits the structure of the lifted convex representation by using ADMM to split the problem into subproblems that can be solved in closed form. The θ\theta-update becomes a set of independent linear quadratic tracking problems, and the zz-update becomes parallel projections. We introduced a spectral ball projection for the terminal covariance constraint to reduce computation time.

The proposed algorithm is validated on a 3D hover linearized quadrotor with different waypoint configurations.

NNne(N)n_e(N)\multicolumn{4}{c}{socx (Ours), GPU}\multicolumn{2}{c}{MOSEK, CPU}\multicolumn{2}{c}{SCS+cuDSS, GPU}
time (s)iterationsobj. err.Pviol/ϵP_{\mathrm{viol}}/\epsilontime (s)speed-uptime (s)speed-up
402520.38±0.020.38 \pm0.02377±20377 \pm202×10−72 \times10^{-7}1.0002.9±0.12.9 \pm0.17.27.2–7.9×7.9\times26±1126 \pm114747–104×104\times
804920.66±0.060.66 \pm0.06483±53483 \pm5310−710^{-7}1.00014.9±1.514.9 \pm1.52020–27×27\times179±79179 \pm79145145–369×369\times
1609722.20±0.382.20 \pm0.38750±135750 \pm1352×10−52 \times10^{-5}1.00291.7±5.591.7 \pm5.53737–53×53\times1976±20771976 \pm2077199199–2224×2224\times
2401,4524.33±0.304.33 \pm0.30798±57798 \pm5710−410^{-4}1.002311.7±82.9311.7 \pm82.95757–98×98\times3848±21233848 \pm2123386386–1342×1342\times
3201,9327.10±0.267.10 \pm0.26808±31808 \pm31–1.000out of memory (50GB cap)5789±16335789 \pm1633576576–1085×1085\times
11206,732144.54±55.48144.54 \pm55.481364±5231364 \pm523––out of memory (50GB cap)time out (10,000s cap)

Table 3. 3D quadrotor at tolerance 10−510^{-5}: mean ±\pm max. deviation over the three setups of Figure 2.

Variant\multicolumn{3}{c}{time (ms)}\multicolumn{3}{c}{scratch memory (MB)}complexity
N=80N=80160320N=80N=80160320
θ\theta-update: per iteration
Parallel LQT (Ours)1.132.628.1739124390
Sequential LQT4.489.1620.66840159
Condensed KKT2.469.5461.19101343
Per-column loop7.7830.63120.721887473768
Terminal-LMI projection: per call
Gram eigh (Ours)0.390.420.41O(nx2W+n3)O(n_x^2 W + n_3)
SVD0.941.050.98O(n2W)O(n^2 W)
Schur PSD8.7922.0388.64O((nx+W)3)O((n_x + W)^3)

Table 4. Ablations on the helix setup at tolerance 10−510^{-5}.

Compared to convex solvers for the same problem, we observe speed-ups of up to 98×98\times compared to MOSEK and 2224×2224\times compared to SCS at 10−510^{-5} tolerance. The proposed approach can also solve for longer horizons of up to N=1120N = 1120, compared with N=320N = 320 for the baselines. Additionally, we presented ablations of the θ\theta-update and the zz-update to further isolate components of the observed speed-ups.

References

  1. [1]S. M. LaValle, Planning algorithms. New York: Cambridge University Press, 2006.
  2. [2]Y. K. Nakka, A. Liu, G. Shi, A. Anandakumar, Y. Yue, and S.-J. Chung, “Chance-Constrained Trajectory Optimization for Safe Exploration and Learning of Nonlinear Systems,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 389–396, Apr. 2021.arxiv.org/abs/2005.04374
  3. [3]Y. K. Nakka and S.-J. Chung, “Trajectory Optimization for Chance-Constrained Nonlinear Stochastic Systems,” in 2019 IEEE 58th Conference on Decision and Control (CDC), Dec. 2019, pp. 3811–3818.DOI
  4. [4]Y. K. Nakka and S. J. Chung, “Trajectory optimization of chance-constrained nonlinear stochastic systems for motion planning under uncertainty,” IEEE Transactions on Robotics, vol. 39, no. 1, pp. 203–222, Feb. 2023.arxiv.org/abs/2106.02801
  5. [5]H. Yu and Y. Chen, “A Gaussian Variational Inference Approach to Motion Planning,” IEEE Robotics and Automation Letters, vol. 8, no. 5, pp. 2518–2525, May 2023.DOI
  6. [6]F. Liu, G. Rapakoulias, and P. Tsiotras, “Optimal Covariance Steering for Discrete-Time Linear Stochastic Systems,” IEEE Transactions on Automatic Control, vol. 70, no. 4, pp. 2289–2304, Apr. 2025.arxiv.org/abs/2211.00618
  7. [7]G. Rapakoulias and P. Tsiotras, “Discrete-Time Optimal Covariance Steering via Semidefinite Programming,” in 2023 62nd IEEE Conference on Decision and Control (CDC), Dec. 2023, pp. 1802–1807.arxiv.org/abs/2302.14296
  8. [8]J. Pilipovsky and P. Tsiotras, “Computationally Efficient Chance Constrained Covariance Control with Output Feedback,” in 2024 IEEE 63rd Conference on Decision and Control (CDC), Dec. 2024, pp. 677–682.arxiv.org/abs/2310.02485
  9. [9]I. M. Balci and E. Bakolas, “Constrained minimum variance and covariance steering based on affine disturbance feedback control parameterization,” International Journal of Robust and Nonlinear Control, vol. 34, pp. 7332 – 7370, 2024.DOI
  10. [10]T. Doan and Y. Kumar Nakka, “Exact and approximate convex reformulation of linear stochastic optimal control with chance constraints,” IEEE Control Systems Letters, vol. 10, pp. 787–792, 2026.
  11. [11]G. Bravo-Palacios, J. Zhang, Z. Pestrikov, B. Plancher, and T. Lew, “Turbompc: Fast, scalable, and differentiable model predictive control on the gpu,” 2026. [Online]. Available: https://arxiv.org/abs/2606.24039
  12. [12]J. Viljoen, F. Haffner, M. Tomizuka, and N. Mehr, “Scaling nonlinear optimization: Many problems one gpu,” 2026. [Online]. Available: https://arxiv.org/abs/2606.26341
  13. [13]S. H. Jeon, S. Hong, H. J. Lee, C. Khazoom, and S. Kim, “Cusadi: A gpu parallelization framework for symbolic expressions and optimal control,” IEEE Robotics and Automation Letters, vol. 10, no. 2, pp. 899–906, 2025.arxiv.org/abs/2408.09662
  14. [14]S. Särkkä and Á. F. García-Fernández, “Temporal parallelization of dynamic programming and linear quadratic control,” IEEE Transactions on Automatic Control, vol. 68, no. 2, pp. 851–866, 2023.DOI
  15. [15]L. Amatucci, J. Sousa-Pinto, G. Turrisi, D. Orban, V. Barasuol, and C. Gemini, “Primal-dual ilqr for gpu-accelerated learning and control in legged robots,” IEEE Robotics and Automation Letters, vol. 11, no. 1, pp. 1010–1017, 2026.DOI
  16. [16]K. Okamoto, M. Goldshtein, and P. Tsiotras, “Optimal Covariance Control for Stochastic Systems Under Chance Constraints,” IEEE Control Systems Letters, vol. 2, no. 2, pp. 266–271, Apr. 2018.arxiv.org/abs/1804.02829
  17. [17]J. Fang and G. Chou, “Safe Large-Scale Robust Nonlinear MPC in Milliseconds via Reachability-Constrained System Level Synthesis on the GPU,” in Proceedings of Robotics: Science and Systems, Sydney, Australia, July 2026.arxiv.org/abs/2604.07644
  18. [18]J. Wang, A. T. Abdul, and E. Theodorou, “cuNRTO: GPU-Accelerated Nonlinear Robust Trajectory Optimization,” in Proceedings of Robotics: Science and Systems, Sydney, Australia, July 2026.arxiv.org/abs/2603.02642
  19. [19]Y. Chen, D. Tse, P. Nobel, P. Goulart, and S. Boyd, “Cudacable: Gpu acceleration for a conic optimization solver,” 2024. [Online]. Available: https://arxiv.org/abs/2412.19027
  20. [20]B. O’Donoghue, E. Chu, N. Parikh, and S. Boyd, “Conic optimization via operator splitting and homogeneous self-dual embedding,” Journal of Optimization Theory and Applications, vol. 169, no. 3, pp. 1042–1068, June 2016. [Online]. Available: http://stanford.edu/~boyd/papers/scs.htmlarxiv.org/abs/1312.3039
  21. [21]MOSEK ApS, MOSEK Optimizer API for Python 11.2.7, 2025. [Online]. Available: https://docs.mosek.com/11.2/pythonapi/index.html
  22. [22]M. Schubiger, G. Banjac, and J. Lygeros, “GPU acceleration of ADMM for large-scale quadratic programming,” Journal of Parallel and Distributed Computing, vol. 144, pp. 55–67, 2020.DOI
  23. [23]A. Mesbah, “Stochastic model predictive control: An overview and perspectives for future research,” IEEE Control Systems Magazine, vol. 36, no. 6, pp. 30–44, 2016.DOI
  24. [24]J. Bradbury, R. Frostig, P. Hawkins, M. J. Johnson, Y. Katariya, C. Leary, D. Maclaurin, G. Necula, A. Paszke, J. VanderPlas, S. Wanderman-Milne, and Q. Zhang, “JAX: composable transformations of Python+NumPy programs,” 2018. [Online]. Available: http://github.com/jax-ml/jax
  25. [25]G. Belloch, “Scans as primitive parallel operations,” IEEE Transactions on Computers, vol. 38, no. 11, pp. 1526–1538, 1989.
  26. [26]S. Boyd, N. Parikh, E. Chu, B. Peleato, and J. Eckstein, “Distributed optimization and statistical learning via the alternating direction method of multipliers,” Foundations and Trends in Machine Learning, vol. 3, no. 1, pp. 1–122, 07 2011. [Online]. Available: https://doi.org/10.1561/2200000016
  27. [27]S. Diamond and S. Boyd, “CVXPY: A Python-embedded modeling language for convex optimization,” Journal of Machine Learning Research, vol. 17, no. 83, pp. 1–5, 2016.

Paper details

vlnc:W_3gw4dn8f
Version
v1 · latest
Published
Fresh — added to the Hub within the last 30 days
Added
License
CC BY 4.0
Usage

views · 0

downloads · 0

A view counts once per browser per day, a download each time the PDF is requested.

  • Engineering

Not recorded for this paper

View statement
Looking for related papers…
Contents