Free Final Time Adaptive Mesh Covariance Steering via Sequential Convex Programming

1. Problem Statement

The paper addresses the Free-Final-Time Covariance Steering (FFT-CS) problem for nonlinear stochastic differential equations (SDEs). The objective is to design a feedback control policy that:

• Steers the system state from a given initial Gaussian distribution to a target terminal Gaussian distribution.
• Minimizes the final time ($t_f$) of the trajectory.
• Satisfies chance constraints (probabilistic bounds) on the state and control inputs throughout the trajectory.
• Handles systems with both additive and multiplicative noise (where diffusion depends on the state and/or control).

Key Challenge: Existing Sequential Convex Programming (SCP) methods for covariance steering typically assume a fixed time mesh and often "freeze" the diffusion term during linearization. This approach fails to accurately capture the dynamics of multiplicative noise, especially when the final time is a decision variable, leading to poor covariance propagation and potential constraint violations.

2. Methodology

The authors propose a Sequential Convex Programming (SCP) framework, specifically an Iterative Covariance Steering (iCS) algorithm, that simultaneously optimizes the control policy and the temporal discretization mesh.

A. Time Normalization and Adaptive Mesh

To handle the free final time, the continuous-time horizon $[0, t_f]$ is mapped to a fixed normalized interval $\tau \in [0, 1]$.

• A time-dilation variable $\sigma(\tau) = dt/d\tau$ is introduced.
• The dynamics are rewritten in terms of $\tau$, where the drift scales by $\sigma$ and the diffusion scales by $\sqrt{\sigma}$.
• The horizon is discretized into $N$ intervals with fixed normalized step sizes $\Delta \tau$, but the physical time steps $\Delta t_k$ vary based on the optimization of per-interval dilation variables $\{\sigma_k\}$. This creates an adaptive mesh that concentrates time steps where uncertainty grows rapidly.

B. Exact First-Order Diffusion Linearization

A critical innovation is the treatment of multiplicative noise. Unlike prior works that freeze the diffusion matrix at the reference trajectory (ignoring its dependence on state/control), this method:

• Derives an exact first-order Itô linearization of the SDE, preserving the coupling between the state, control, and the time-dilation variable $\sigma$.
• This results in a local linear SDE where the diffusion term remains multiplicative (dependent on the state and control).

C. Tractable Discretization and Error Bounds

To make the problem solvable via convex optimization:

• The authors employ a Zero-Order Hold (ZOH) assumption on the control and time-dilation variables.
• They approximate the stochastic integral by freezing the state at the beginning of the interval and projecting the integrand onto a constant matrix.
• Theorem 1 provides rigorous one-step mean-square error bounds, showing that the proposed method has an error of order $O(\Delta \tau^2)$ (dominated by the freezing approximation), which is an order of magnitude better than methods that freeze the diffusion before linearization (which incur $O(\Delta \tau)$ error).

D. Convex Reformulation (SCvx)

The resulting non-convex problem is transformed into a sequence of convex subproblems:

1. Moment Propagation: The mean and covariance dynamics are derived. The covariance recursion involves non-convex terms coupling the covariance matrix and the feedback gain.
2. Lossless Relaxations:
   • Lifting: New variables are introduced (e.g., $U_k = K_k \Sigma_k$) to linearize the coupling.
   • Schur Complement: Semidefinite programming (SDP) constraints are used to convexify the quadratic terms (e.g., $Y_k \succeq U_k \Sigma_k^{-1} U_k^T$).
   • Theorem 3 proves that under suitable regularization, these relaxations are lossless, meaning the solution to the relaxed convex problem is also the solution to the original non-convex problem.
3. Chance Constraints: Distributionally Robust Chance Constraints (DR-CC) are used, relying only on the first two moments. These are reformulated into Second-Order Cone (SOC) constraints using Boole's inequality and auxiliary variables.

E. Algorithm (FFT-iCS SCvx*)

The algorithm iterates between:

1. Linearizing the time-scaled SDE around a reference trajectory.
2. Solving the convex SDP subproblem (minimizing final time + regularization + penalties).
3. Updating the reference trajectory and trust-region parameters using an augmented Lagrangian approach (SCvx* logic) to ensure convergence and feasibility.

3. Key Contributions

1. First Free-Final-Time Formulation for Nonlinear SDEs: The paper presents the first framework to jointly optimize the control policy and the temporal mesh for nonlinear stochastic systems with multiplicative noise.
2. Exact Diffusion Linearization: It introduces a novel linearization technique that retains the first-order dependence of the diffusion term on the state, control, and time-dilation, significantly improving accuracy over "frozen diffusion" approximations.
3. Rigorous Error Analysis: Theoretical bounds are derived proving that the proposed discretization scheme reduces the covariance propagation error by an order of magnitude compared to existing methods.
4. Lossless Convex Relaxation: The paper demonstrates that the non-convex covariance steering constraints can be solved exactly via SDP relaxations when appropriate regularization is applied.

4. Numerical Results

The method was validated on a nonlinear double-integrator system with:

• Nonlinear drag forces ($v \cdot |v|$).
• Velocity-dependent (multiplicative) diffusion.

Findings:

• Additive Noise Case: The algorithm successfully minimized the final time by adaptively allocating time steps, converging within 62 iterations for a representative case.
• Multiplicative Noise Case: When compared to a standard "frozen diffusion" approach:
  • The proposed method converged faster (16 iterations vs. 33).
  • Accuracy: The proposed method achieved a 95% improvement in covariance propagation accuracy. The terminal covariance error was $-0.002$ (slightly under the target) compared to $+0.04$ (significant over-estimation) for the frozen method.
  • Feasibility: Monte Carlo simulations confirmed that the proposed policy satisfied the terminal covariance and chance constraints, whereas the frozen method's linearization led to significant discrepancies in the true nonlinear system behavior.

5. Significance

This work bridges a critical gap in stochastic optimal control by enabling real-time capable, high-precision guidance for systems where uncertainty scales with the state or control (common in aerospace, robotics, and chemical processes). By integrating adaptive time meshing with exact multiplicative noise handling, the method allows for:

• More efficient mission planning (minimizing time).
• Safer operations (accurate uncertainty quantification).
• Robustness in highly nonlinear and stochastic environments where previous convex methods would fail or produce overly conservative solutions.

The framework is particularly relevant for spacecraft proximity operations, low-thrust trajectory design, and any application requiring precise distribution shaping under time-varying uncertainty.