🔢 mathematics

Square Root Gauss-Newton iLQR

This paper presents a numerically robust and algorithmically simple Square Root Gauss-Newton iLQR formulation that leverages the weighted least-squares structure of the cost function to enable a backward pass based on single QR-decompositions, thereby improving conditioning and stability, particularly within augmented Lagrangian frameworks for constrained trajectory optimization.

Original authors: Maximilian Haas-Heger, Jur van den Berg

Published 2026-09-21

📖 6 min read🧠 Deep dive

CC BY 4.0

Original authors: Maximilian Haas-Heger, Jur van den Berg

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Robots, autonomous cars, and spacecraft all share a fundamental challenge: they must move from one place to another while obeying the laws of physics and avoiding obstacles. To do this, they rely on a mathematical process called trajectory optimization, which acts like a super-powered navigator. This navigator constantly calculates the best possible path by testing countless variations of speed and direction, looking for the route that uses the least energy or time. When the world is simple and predictable, standard math tools can solve this puzzle perfectly. But the real world is messy. Robots have complex joints, cars slide on wet roads, and spacecraft must dodge debris. In these situations, the math becomes non-linear and difficult, requiring the computer to make a series of educated guesses, refine them, and repeat the process until it finds a solution that works. This iterative process is the backbone of modern motion planning, allowing machines to perform delicate tasks like assembling parts or landing on other planets.

However, as these machines become more capable, they are asked to operate under stricter rules. A robot arm might need to avoid hitting a human hand, or a drone might need to stay within a narrow corridor. To enforce these rules, computers add heavy penalties to their calculations whenever a path comes close to breaking a constraint. While this forces the robot to obey, it also creates a hidden danger: the numbers the computer uses to find the path can become so wildly different in size that the machine loses its ability to distinguish between them. It is like trying to measure the width of a single hair while standing next to a mountain; the tiny detail gets lost in the noise of the massive scale. When this happens, the computer's calculations become unstable, and the robot may fail to find a path at all, or worse, calculate a path that looks perfect on paper but is physically impossible.

In a recent study, researchers Maximilian Haas-Heger and Jur van den Berg from Waabi Innovation Inc. tackled this problem of numerical instability. They focused on a specific algorithm known as the iterative Linear Quadratic Regulator, or iLQR, which is widely used to plan these complex paths. The team discovered that by changing how the algorithm handles its internal math, they could make it significantly more robust without slowing it down. Their work centers on a technique called a "square root" formulation. In standard computing, the algorithm keeps track of a large table of numbers representing the cost of every possible move. When constraints are tight, this table becomes difficult to read. The researchers realized that instead of tracking the full table, the algorithm could track a simpler, triangular version of it, much like keeping only the essential ingredients of a recipe rather than the entire pantry.

The key to their success lies in the way they simplified the problem. They treated the robot's goal as a weighted sum of errors, similar to how a photographer might focus on minimizing the blur in a photo by adjusting specific settings. By using a method known as Gauss-Newton, which approximates complex curves with straight lines, they ensured that the mathematical structure of the problem remained stable. This allowed them to replace the heavy, unstable calculations with a lighter, more precise operation called a QR decomposition. Think of this operation as a way of organizing a messy pile of data into a neat, orderly stack where every piece fits perfectly without any gaps or overlaps. This new approach allows the computer to maintain its precision even when the penalty for breaking a rule becomes enormous.

The researchers tested their new method on a challenging scenario involving two robotic arms working together to carry a rigid object. The arms had to move the object from a starting point to a goal while keeping their grips at a fixed distance from each other, a constraint that required the computer to balance multiple competing demands. They ran simulations where the penalty for violating this distance rule was increased to extreme levels, far beyond what a standard computer would normally handle. In these tests, the traditional method began to fail, losing accuracy and eventually producing nonsense results as the penalty grew. The new square root method, however, held its ground. It continued to produce accurate paths even when the penalty was so large that the standard method had completely broken down. In fact, the new approach retained six to seven significant digits of accuracy in standard computer arithmetic where the old method had lost almost all of them.

This improvement is not just a theoretical win; it translates directly into reliability for real-world machines. The study showed that the new method could drive the optimization process much further, allowing robots to satisfy constraints with a level of tightness that was previously impossible. While the standard method might give up when asked to be precise to one part in a million, the new method could push that precision to one part in a hundred billion. The researchers noted that this gain in precision comes with almost no extra cost in computing time, adding less than three percent to the total time required to solve a problem. This means that engineers can use the new method as a drop-in replacement for the old one, instantly making their robots more capable of handling difficult, high-stakes tasks.

The paper also addresses why this specific improvement has not been widely adopted before. While similar "square root" techniques have been used for decades in navigation systems like the Kalman filter, they have been harder to apply to trajectory optimization because the math is more complex. Previous attempts to create a square root version of iLQR were either too complicated to use or numerically inferior. The authors argue that their approach is different because it leverages the specific structure of the problem to simplify the math, reducing each step to a single, elegant operation. They suggest that for any robot planning a path under strict constraints, there is no reason not to use this more stable method, just as engineers would not choose an unstable navigation system for a spacecraft.

Ultimately, the work demonstrates that the limits of robot movement are often defined not by the laws of physics, but by the limits of the computer's ability to calculate them. By refining the way these calculations are performed, the researchers have removed a significant barrier, allowing machines to navigate more complex and dangerous environments with greater confidence. The study confirms that with the right mathematical tools, we can push the boundaries of what autonomous systems can achieve, ensuring they remain precise and reliable even when the stakes are highest.

Technical Summary: Square Root Gauss-Newton iLQR

Problem Formulation

The paper addresses the problem of nonlinear trajectory optimization for discrete-time dynamical systems. The objective is to find a sequence of control inputs $u_0, \dots, u_{\ell-1}$ that steers a system $x_{t+1} = f_t(x_t, u_t)$ from an initial state to a final state while minimizing a total cost function. The authors focus specifically on cost functions expressible as weighted sums of squared residuals (Gauss-Newton structure):
$c_t(x_t, u_t) = \frac{1}{2} r_t(x_t, u_t)^T W_t r_t(x_t, u_t)$
where $r_t$ is a vector of residuals and $W_t$ is a positive-definite weight matrix. This formulation is common in robotics and autonomous driving, particularly when handling constraints via an augmented Lagrangian framework. In such frameworks, large penalty parameters $\mu$ are used to enforce constraints, which significantly degrades the conditioning of the optimization problem, leading to numerical instability in standard solvers.

Methodology

Standard Gauss-Newton iLQR

The Iterative Linear Quadratic Regulator (iLQR) solves this problem by iteratively linearizing the dynamics and quadratizing the cost around a nominal trajectory. In the standard Gauss-Newton variant, the cost is approximated by linearizing the residual vector $r_t$ rather than performing a full second-order expansion of the cost function. This yields an augmented Hessian matrix $C_t = R_t^T W_t R_t$ , which is positive-semidefinite (PSD) by construction. The algorithm proceeds via a backward pass (computing optimal feedback gains and cost-to-go matrices) and a forward pass (simulating the system with the new policy).

The Square Root Formulation

The core contribution of this paper is a Square Root Gauss-Newton iLQR algorithm. Instead of propagating the full cost-to-go Hessian matrix $S_t$ , the algorithm propagates its upper-triangular Cholesky factor $\sqrt{S_t}$ (where $S_t = \sqrt{S_t}^T \sqrt{S_t}$ ).

The derivation relies on two key insights:

Global PSD Property: Due to the Gauss-Newton structure, not only is the Hessian block of the cost-to-go matrix PSD, but the entire augmented cost-to-go matrix $Q_t$ is PSD.
QR-Decomposition Efficiency: The matrix $Q_t$ can be expressed as $M_t^T M_t$ , where $M_t$ is constructed from the square roots of the weights, the residual Jacobians, and the square root of the next time step's cost-to-go factor.

The backward pass is simplified to a single QR-decomposition of the matrix $M_t$ :
$M_t = \begin{bmatrix} \sqrt{W_t} R_t \\ \sqrt{S_{t+1}} F_t \end{bmatrix}$
Performing a QR-decomposition ( $M_t = QU$ ) yields the upper-triangular factor $U$ , which directly encodes the Cholesky factor of $Q_t$ . The feedback gain $L_t$ and the next cost-to-go factor $\sqrt{S_t}$ are extracted directly from the blocks of $U$ . This approach avoids explicitly forming the squared matrices (like $Q_t$ or $S_t$ ), thereby preventing the amplification of rounding errors.

For constrained problems using the augmented Lagrangian method, the constraint Jacobians and penalty terms are simply appended as an additional row block to $M_t$ , maintaining the same computational structure.

Key Contributions

Numerical Stability: By propagating Cholesky factors rather than full matrices, the algorithm effectively doubles the number of reliable significant digits. The condition number of the Cholesky factor scales with $\sqrt{\mu}$ rather than $\mu$ , directly mitigating the numerical degradation caused by large penalty parameters in constrained optimization.
Algorithmic Simplicity: Unlike previous square root formulations for iLQR or Differential Dynamic Programming (DDP), which were either algorithmically complex or required multiple decompositions and rank-one updates, this method reduces each backward pass step to a single QR-decomposition.
Computational Efficiency: The overhead of the square root formulation is minimal. The computational complexity increases by at most a factor of 2 (the classic cost of QR over normal equations) for the leading term dependent on the residual dimension. In practical scenarios (e.g., bimanual manipulation), the total computation time increases by less than 3%.
Robustness to Singularity: The method guarantees numerical positive-semidefiniteness by construction. While standard solvers may fail entirely when the Hessian becomes indefinite due to rounding errors, the square root variant degrades gracefully, allowing for health checks based on the diagonal entries of the triangular factor.

Experimental Results

The authors validated the method on a constrained trajectory optimization problem involving two 3-link robot arms cooperatively transporting an object.

Precision Comparison: When tested against a high-precision reference (50 decimal digits), the standard iLQR (using double precision, f64) lost approximately one significant digit for every decade increase in the penalty parameter $\mu$ . At $\mu = 10^{16}$ , the standard method failed completely. In contrast, the square root f64 variant retained six to seven significant digits even at $\mu = 10^{20}$ .
Single Precision (f32): The advantage was even more pronounced in single precision. The standard f32 solver failed at $\mu = 10^7$ , whereas the square root f32 solver remained functional up to $\mu = 10^{15}$ , providing five to six extra decades of stability.
Convergence: In a full augmented Lagrangian loop, the square root solver successfully converged from 24 randomized initial trajectories to a constraint tolerance of $10^{-8}$ , while the standard solver failed at $10^{-7}$ . The square root method did not find a "better" optimum but allowed the outer loop to drive the constraints tighter before numerical noise caused the backward pass to fail.

Significance and Claims

The paper claims that for trajectory optimization problems that can be formulated with weighted sums-of-squares costs (a broad class including many robotics applications), the Square Root Gauss-Newton iLQR should be the default choice over standard iLQR. The authors draw a parallel to the widespread adoption of square root Kalman filters in estimation, noting that the dual nature of estimation and control suggests similar benefits.

The significance lies in the combination of elegance and robustness. The method eliminates the need for ad-hoc regularization techniques (like Levenberg-Marquardt damping) often required to maintain positive-definiteness in standard iLQR. The authors assert that there are "no downsides" to using this formulation in production systems where the Gauss-Newton structure applies, as it offers superior numerical conditioning with negligible computational overhead. The paper concludes that the success of square root methods in estimation should extend to trajectory optimization, provided the problem structure allows for the Gauss-Newton approximation.