Direct Trajectory Optimization and Costate Estimation via an Orthogonal Collocation Method

A pseudospectral method, called the Gauss pseudospectral method, for solving nonlinear optimal control problems is presented. In the method presented here, orthogonal collocation of the dynamics is performed at the Legendre-Gauss points. This form of orthogonal collocation leads a nonlinear programming problem (NLP) whose Karush-Kuhn-Tucker (KKT) multipliers can be mapped to the costates of the continuous-time optimal control problem. In particular the Legendre-Gauss collocation leads to a costate mapping at the boundary points. The method is demonstrated on an example problem where it is shown that highly accurate costates are obtained. The results presented in this paper show that the Gauss pseudospectral method is a viable apprach for direct trajectory optimization and costate estimation.