Square Root Gauss-Newton iLQR

Abstract

The iterative Linear Quadratic Regulator (iLQR) is a widely used algorithm for nonlinear trajectory optimization. At each iteration, it solves a local linear-quadratic approximation of the problem via dynamic programming, propagating a quadratic cost-to-go function. If the Hessian of the cost-to-go approximation is positive-semidefinite, one can derive a square root formulation of iLQR that propagates its Cholesky factor instead. This offers significant numerical advantages - much as square root Kalman filters improve upon their conventional counterparts - particularly when iLQR is used within an augmented Lagrangian framework for handling constraints, where large penalties degrade conditioning. Previous square root formulations of iLQR and related algorithms exist, but they are either numerically suboptimal, algorithmically complex, or both. In this paper, we show that the key to an effective square root formulation lies in the Gauss-Newton (weighted least-squares) structure of the cost function: this yields a positive semidefiniteness property that extends beyond the Hessian to the full augmented cost-to-go matrix, and enables a backward pass of remarkable simplicity in which each step reduces to a single QR-decomposition, from which the feedback gain and propagated Cholesky factor are extracted directly.