Partitioning and QR factorization to avoid numerical ill-conditioning of penalty functions for constrained nonlinear programming and control

Conventional implementation of the penalty function methods results in numerical inaccuracies in the Hessian matrix and its inverse as the penalty parameter approaches its limit point. The authors describe a procedure for avoiding these numerical difficulties by partitioning the Hessian matrix into two numerically well conditioned submatrices. QR factorization is used to enhance the numerical accuracy of the calculation of the Hessian matrix and its inverse. Proofs of the convergence of the procedure when the penalty parameter approaches its limit are presented. By partitioning the Hessian matrix, it is possible to show that the procedure generates a bounded series of inverse Hessian matrices even though the Hessian itself is unbounded as the penalty parameter approaches its limit. The QR partition penalty function procedure is combined with the conventional unconstrained differential dynamic programming scheme to develop a new constrained differential dynamic programming algorithm for solving the general discrete-time constrained nonlinear optimal control problem