Intrinsic SE(3) Covariance-Steered World Model
Implementation & benchmark of arXiv:2608.04114 — Intrinsic Stochastic Successive Convexification on SE(3) for Chance Constrained 6-DOF Rendezvous
Source paper: Intrinsic Stochastic Successive Convexification on SE(3) for Chance Constrained 6-DOF Rendezvous arXiv:2608.04114 ⓘ · analyzed Aug 31, 2026
AI-generated research hypothesis, automatically tested. Not peer-reviewed.
Idea description
Replace a Euclidean position-plus-rotation recurrent state with an SE(3)-valued latent pose and predict six-dimensional algebra increments rather than directly regressing a rotation matrix or Euler angles. Jointly propagate a pose covariance and penalize Gaussian chance-constraint violations, so the model learns both a nominal trajectory and feedback-like uncertainty contraction.
Formulas
Mathematical statement
A pose is represented as $T=\begin{bmatrix}R&t\\0&1\end{bmatrix}\in SE(3)$, where $R\in SO(3)$ and $t\in\mathbb{R}^3$. A twist $\xi=[\omega^\top,v^\top]^\top\in\mathbb{R}^6$ is mapped to $se(3)$ by $\xi^\wedge=\begin{bmatrix}[\omega]_\times&v\\0&0\end{bmatrix}$, and the intrinsic recurrent update is $T_{k+1}=T_k\exp(\xi_k^\wedge)$. The exponential couples rotation and translation through $t_{k+1}=R_kJ_l(\omega_k)v_k$ rather than adding them independently. For local uncertainty $\delta\xi_k\sim\mathcal{N}(0,\Sigma_k)$, use the linearized stochastic dynamics $\delta\xi_{k+1}=A_k\delta\xi_k+B_k\epsilon_k$, with covariance recursion $\Sigma_{k+1}=A_k\Sigma_kA_k^\top+B_kQ_kB_k^\top$; $A_k$ is the Jacobian of the learned intrinsic transition, $B_k$ is the noise Jacobian, and $Q_k\succeq0$ is process-noise covariance. For a locally linearized scalar constraint $g(T)\approx g(\bar T)+a^\top\delta\xi$, the Gaussian chance constraint is $g(\bar T)+\kappa_\alpha\sqrt{a^\top\Sigma a}\le0$, where $\kappa_\alpha=\Phi^{-1}(1-\alpha)$ and $\alpha$ is the allowed violation probability.
Implementation notes
1) Integration point: use this as the latent-state transition in a small recurrent world model, pose forecaster, visual-odometry network, or robot-policy model. Maintain the hidden pose as a 4x4 SE(3) matrix, while the network outputs a six-vector twist mean $\mu_k$ and optionally a lower-triangular covariance factor $L_k$ with $Q_k=L_kL_k^\top+\varepsilon I$. Other hidden features can remain Euclidean. 2) Pseudocode: initialize $T_0$ from data and $\Sigma_0$ from empirical pose noise; at each step compute $(\mu_k,L_k,h_k)=\mathrm{NN}_\theta(\log(T_k^{-1}T_k^{obs}),z_k)$, set $T_{k+1}=T_k\exp(\mu_k^\wedge)$, estimate the transition Jacobian $A_k$ by autodiff or finite differences of the six-dimensional log-coordinate transition, form $Q_k=L_kL_k^\top+10^{-5}I$, update $\Sigma_{k+1}=A_k\Sigma_kA_k^\top+Q_k$, and add the task and chance-penalty terms above. Backpropagate through the matrix exponential; use Rodrigues formulas with a Taylor branch for $\|\omega\|<10^{-4}$. 3) Computed from the paper's mechanism: the wedge/vee representation, exponential retraction, intrinsic translation-rotation coupling, covariance propagation, and Gaussian safety margin. Estimated empirically: $A_k$ from autodiff, $Q_k$ from the learned noise head, and constraint gradients $a_{j,k}=\partial g_j/\partial\delta\xi$. 4) First cheap experiment: train a two-layer GRU world model on KITTI odometry, EuRoC, or synthetic screw-motion sequences with 20-step rollout loss. Compare direct Euclidean translation plus quaternion regression, deterministic SE(3) increments, and SE(3) increments plus covariance steering. Use held-out trajectories with perturbed initial poses and evaluate rotation/translation rollout error and empirical violation probability for a synthetic docking corridor. The quantitative prediction is that the intrinsic model will show no orthogonality or quaternion-normalization drift, while the covariance model's measured violation rate should approach target $\alpha$ and remain below the deterministic model under matched nominal error. Sweep the learned step scale $s$ multiplying $\mu_k$; instability or rapidly increasing rollout covariance should occur when the largest estimated Jacobian eigenvalue satisfies $\rho(A_k)\gtrsim1$, whereas covariance-steered training should produce a monotone reduction in $\operatorname{tr}(\Sigma_k)$ on stable trajectories.
Verification
This idea has not been verified yet.
Verification happens in two stages: Stage 1 — a mechanism check on a toy system confirms the claimed mathematical phenomenon reproduces; Stage 2 — a benchmark implements the idea on a real (small) neural network task and compares it against a tuned baseline over 8 paired seeds with a permutation test.
Artifacts
Artifacts unavailable.