A generalization of the Riccati recursion for equality-constrained linear quadratic optimal control

This paper introduces a generalization of the well-known Riccati recursion for solving the discrete-time equality-constrained linear quadratic optimal control problem. The recursion can be used to compute problem solutions as well as optimal feedback control policies. Unlike other tailored approaches for this problem class, the proposed method does not require restrictive regularity conditions on the problem. This allows its use in nonlinear optimal control problem solvers that use exact Lagrangian Hessian information. We demonstrate that our approach can be implemented in a highly efficient algorithm that scales linearly with the horizon length. Numerical tests show a significant speed-up of about one order of magnitude with respect to state-of-the-art general-purpose sparse linear solvers. Based on the proposed approach, faster nonlinear optimal control problem solvers can be developed that are suitable for more complex applications or for implementations on low-cost or low-power computational platforms. The implementation of the proposed algorithm is made available as open-source software.


INTRODUCTION
The equality-constrained linear quadratic optimal control problem, or constrained LQ problem for short, is a generalization of the classical LQ problem 1 that supports affine stagewise mixed input-state equality constraints.Here, stagewise means that the constraints only relate inputs and states of a specific stage, or time step, of the control horizon.An important type of these constraints are initial and terminal stage constraints, resulting in a two-point boundary value optimization problem.Constrained LQ problems are encountered in linear quadratic optimal control applications but also as a subproblem in many nonlinear optimal control problem solvers.In these nonlinear optimization solvers, at each iteration, the problem is approximated by a constrained LQ problem, resulting in a series of constrained LQ subproblems to be solved.Two classes of nonlinear optimal control solver algorithms can be distinguished.The first class are algorithms based on optimal feedback control policies.This class includes Differential Dynamic Programming (DDP) and the related iterative Linear Quadratic Regulator (iLQR).Given a local linear approximation of system dynamics and a linear quadratic approximation of the problem Lagrangian, these algorithms compute an optimal feedback control policy at each stage in the backward pass.These calculated feedback control policies are then used to compute the next iterate in the forward pass.This process is repeated until a local minimum of the cost function is found.Iterative LQR is a DDP-variant that uses a Gauss-Newton approximation of the Lagrangian Hessian.Hence, it can be seen as if the second-order information of the dynamics and stagewise equality constraints are neglected 2 .The second class of algorithms are Newton-type algorithms.These algorithms directly optimize for the open-loop trajectories of the linearized problem model to compute the next iterate.These algorithms include Direct Single Shooting and Direct Multiple Shooting.The latter is known to have superior convergence properties over the former 3 and is able to be initialized from dynamically infeasible estimates of the solution.Stagewise inequality constraints have been successfully incorporated by using penalty methods and barrier (interior point) methods that preserve the unconstrained LQ problem structure 4 .This means that the classical Riccati recursion can be used without adaptation.To date one of the main shortcomings of nonlinear optimal control problem solvers is the treatment of stagewise equality constraints.Penalty methods have been used as well for equality constraints, but these methods are inexact or require more iterations than imposing the constraints directly.Efficiently solving the constrained LQ problem is of particular importance because it often appears as the most time-consuming step of nonlinear optimal control solver algorithms.Since the constrained LQ problem is a special case of an equality-constrained quadratic programming (QP) problem, its solution can be found using the problem's Karush-Kuhn-Tucker (KKT) optimality conditions.These result in a symmetric and indefinite linear system that can be solved using general-purpose linear solvers.Dense linear solvers do not exploit the particular stagewise structure of the problem, and their computational complexity grows cubically with the control horizon length.Sparse linear solvers, on the contrary, exploit the problem structure and scale linearly with the horizon length.While general-purpose sparse linear solvers do exploit the block-sparse structure of the considered linear system, they do not allow computationally efficient implementations.
Tailored methods for solving the constrained LQ problem have been developed in prior work.Domahidi et al. developed a structure-exploiting range space method that is able to solve a slightly more general problem formulation than the constrained LQ problem 5 .Sideris & Rodriguez proposed a factorization scheme that can cope with stage-wise input-state equality constraints by eliminating the corresponding dual variables 6 .Giffthaler & Buchli showed that projecting the input variables onto the space of inputs that are admissible with respect to the stagewise equality constraints results in a possibly singular optimal control problem.This optimal control problem can then be solved by a modified version of the classical Riccati recursion 2 .Laine & Tomlin applied a scheme where the nullspace of the input part of the stagewise equality constraints is used to modify the Riccati recurison 7 .A shortcoming of all these approaches is that they impose restrictive regularity conditions on the problem, such as a positive definite Lagrangian Hessian.These regularity conditions inhibit their use in many applications, for example in nonlinear optimization algorithms make use of exact Lagrangian Hessian information.In Section 7 we compare our proposed approach to these approaches in detail.
The salient features of our recursion and the contributions of this paper are fourfold: First, we present a generalization of the Riccati recursion that allows stagewise equality constraints.This recursive factorization scheme allows for efficient implementations for both optimal feedback control policy and optimal solution computation.Second, we avoid imposing restrictive regularity conditions on the problem.For example, unlike several other methods 6,8,5 we do not impose positive definiteness of the full-space Hessian, such that our approach is not restricted to Lagrangian Hessian approximations that are guaranteed to be positive definite, and is hence much more flexible in the choice of using exact or approximate Lagrangian Hessian information, in a nonlinear programming algorithm context.Third, our recursion detects ill-posed problems, i.e. problems that do not have a unique minimizer, without additional computational cost.In line-search nonlinear programming algorithms it is common to detect such ill-posed (sub)problems because they do not guarantee a descent direction with respect to the current iterate.Stateof-the-art nonlinear programming solvers such as IPOPT 9 modify the full-space Lagrangian Hessian with an inertia correction while KNITRO 10 switches to a trust-region step in such case.Fourth, we present the recursion step by step from a linear algebra perspective, with a detailed mathematical analysis and discussion.Instead of approaching the problem from a dynamic programming perspective, we look at it as a stagewise factorization of the full KKT system.This allows us to elegantly prove the claims we make for the second and third contribution using only linear algebra.
The structure of this paper is as follows.First, Section 2 discusses the notation and some prerequisites that are necessary throughout the rest of the paper.Then, Section 3 defines the considered constrained LQ problem class.Subsequently, Section 4 gives an overview of the generalized Riccati recursion in a step-by-step fashion.Section 5 discusses some practical implementation aspects and provides the full algorithm for solving the constrained LQ problem.Section 6 compares the performance and accuracy of the proposed method with three general-purpose sparse linear solvers on a set of randomly generated problems and a real-world quadrotor problem.Section 7 compares the proposed recursion with related work, how the approach can also be used to derive optimal feedback control policies and the method's limitations.Finally, Section 8 provides the conclusions and outlook.

KKT system and reduced Hessian
Consider the equality-constrained quadratic programming (QP) problem: with primal optimization variables  ∈ ℝ  , Hessian  ∈   , and constraint Jacobian  ∈ ℝ × , with  ≤ .The first-order necessary conditions for  * to be a solution of (1), state that there is a vector  * ∈ ℝ  such that: in which  is referred to as the Karush-Kuhn-Tucker (KKT) matrix of problem (1) 11 .We refer to the set of equations from the top and bottom block row as, respectively, the stationarity and the constraint equations.In the remainder of the paper we use the following more compact notation for systems of equations like (2): in which we omit the asterisks ( * ).
Definition 1 (Reduced Hessian of a KKT matrix).A reduced Hessian  is equal to the Hessian , projected on the nullspace of the constraint Jacobian : where  is defined as a matrix whose columns span the nullspace of .In case the nullspace of  is empty, the reduced Hessian is defined as an empty matrix.
Note that in the definition above, we speak about a (and not the) reduced Hessian, since reduced Hessians are nonunique as they depend on the choice of the nullspace basis used in .In the remainder of this paper we will omit this detail and speak about the reduced Hessian because it is less confusing and common practice in optimization textbooks.
Theorem 1 (Existence of a unique minimizer).Assuming full row rank of the constraint Jacobian, problem (1) has a unique minimizer if and only if the reduced Hessian of its KKT matrix is positive definite 11 .

Decomposition
Theorem 2. Any real matrix  ∈ ℝ × , with  and  nonzero and  the rank of , can be decomposed as: with   ∈ ℝ × and   ∈ ℝ × invertible matrices.
Proof.Proof by construction.There are several options for constructing such decomposition: • Singular Value Decomposition (SVD) 12 Performing an SVD of  yields: We can obtain a decomposition of the form (4) by choosing   and   as: • LU decomposition with complete pivoting 12 Performing an LU decomposition of  yields: with   ∈ ℙ  ,   ∈ ℙ  permutation matrices,  1 ∈   , an invertible lower triangular matrix,  1 ∈   an invertible upper triangular matrix,  2 ∈ ℝ (−)× and  2 ∈ ℝ ×(−) .The LU decomposition can be used to factor  as:

• Other decompositions
Other decompositions that can be used include QR with column pivoting and the Complete Orthogonal decomposition.
A discussion on these decompositions can be found in Golub et al. 12 .
In the remainder of this paper, if  = 0 or  = 0, we define   =   and   =   , with  0 empty.Furthermore, we omit all linear algebra operations involving empty matrices and we define the product of two matrices  ∈ ℝ ×0 , and  ∈ ℝ 0× as  =  × .

PROBLEM DEFINITION
This paper considers the constrained LQ problem written in the following standard form: for  = 0, … ,  − 1 in (5b) and (5d), and with   (  ) and   (  ,   ) defined as: with state vector and  the horizon length, or number of control intervals.The problem has stagewise quadratic objective function terms   , and affine system dynamics (5b), terminalstate (5c) and stagewise mixed state-input constraints (5d).We refer to Section 4, Equation ( 6) for the KKT system structure of this problem.Apart from linear independence of the constraints (5b)-(5d), the only assumption that is made on the problem's regularity, is that it is well-posed, meaning that it has a unique minimizer.As explained in Section 2.2, this is equivalent to the requirement that the reduced Hessian of the KKT matrix of the problem is positive definite.

APPROACH
In this section the recursive scheme for solving the KKT system of the constrained LQ problem is outlined.For notational brevity, we consider  = 2 in the main text, but in the algorithm environments we always give the general case (i.e. for any ).
The KKT system of problem ( 5) is given by: Here,   ,   and   are the dual optimization variables associated with the terminal-state constraint (5c), the mixed state-input constraints (5d), and the dynamics constraints (5b), respectively.We exploit the block-banded diagonal structure of the KKT matrix through a Riccati-inspired backward substitution followed by a forward substitution 13 .At each step of the backward substitution, a subset of the primal and dual optimization variables are eliminated, starting from the terminal stage.In the forward substitution the primal and dual variables are computed in the reverse order of the backward substitution, resulting in a bottom-up algorithm, starting from the initial stage.As described below, the order of eliminating variables in the backward substitution is chosen in such a way that the top left submatrix of the transformed KKT matrix at the end of the stagewise factorization has the same structure as the KKT matrix at the beginning.As a result, the same steps can be applied recursively for the following, prior in time, stage.This procedure can be repeated until the initial stage is reached, as described in the final paragraph of this section.

Substitution of dynamics.
Eliminate  2 and  2 from the first and third block row, respectively.Substitution of in a transformed KKT system with top left matrix: The matrix [  ,  ,   ] is a concatenation of the mixed state-input constraint of stage  (here  = 1) and the state constraint on  +1 , after substitution of the dynamics, while   is the number of equations represented by this matrix.Symmetric transformation.Using the decomposition explained in Section 2.3, decompose  1, as: with  1 the rank and  1 the number of rows of  1, .Now define ũ1, , λ1 ∈ ℝ  1 , and ũ1, ,  1 ∈ ℝ  1 − 1 and apply the following transformation of variables: ) , (8)   and left-multiply the first and the third block row of ( 7) by  ′ −1 ,1 and  −1 ,1 , respectively.The top left block of the KKT system now becomes: Note that the stagewise constraints are recombined into two subsets.The top subset (9a) can always be satisfied by choosing the appropriate input ũ1, .The bottom subset (9b) represents a constraint that cannot be satisfied by choosing appropriate inputs at the current stage: it puts a constraint on the state  1 .It can be seen as if the transformation of this step transfers this subset of the constaints to the next, prior in time, stage.The columns of ] ′ form a basis for the nullspace of  1, and, hence, ũ1, can be seen as a parameterization of the linear subspace of inputs that are admissible without violating the stagewise constraints.The size of ũ1, and ũ1, is hence equal to the rank and nullity of  1, , respectively.We refer to Table 2 for the dimensions of all submatrices involved in the algorithm.
Substitution of stagewise equality constraints.Eliminate ũ1, and λ1 by substituting ũ1, = G1,  1 + g1 and ) ′ + S′ 1  1 + r1 , the KKT system is transformed to a system with top left matrix: Note that decision variable ũ1, only appears in the Hessian, and not in the constraint Jacobian of the (transformed) KKT system (10).This means that these variables are unconstrained.Since, essentially, we are still solving the original optimization problem (5) it is easy to see that, if the original problem has a unique solution, the optimization problem associated with KKT system (10) should have a unique solution as well.This means that R1 should be positive definite.This fact is used in the next step.For a formal proof we refer to the appendix of this dissertation.Schur complement step.Eliminate ũ1, , by using the Schur complement of R1 , resulting in the system: Because R1 is positive definite, the Cholesky decomposition R1 = Λ 1 Λ ′ 1 , with Λ 1 ∈    − and invertible, can be used to efficiently calculate this Schur complement.If this Cholesky decomposition fails, it means that the problem is ill-posed.This closes the recursion, since the top left submatrix of the transformed KKT matrix now has the same form as the top left submatrix of the original KKT matrix.This means that the previous steps can be applied recursively until all primal and dual optimization variables except for the initial stage are eliminated.Factorization of initial stage.When all primal optimization variables except for  0 are eliminated, the KKT system is transformed into the system: Now we factor the matrix  0 into: Note that  0 has full row rank because linear independence of the constraints is assumed.Hence, there are no zero rows in the middle matrix of the decomposition, and   is equal to the number of rows of  0 :   =  0 −  0 .The -subscript indicates that the quantities are associated with the initial stage.This decomposition gives rise to a transformation of variables: with  0, ∈ ℝ   and  0, ∈ ℝ   −  .After multiplying the first block row with  ′ −1 , and the last block row with  −1 , , the system is transformed to: Now eliminate x0, = h and ) ′ + P .This results in the system: Following the same reasoning as before, P has to be positive definite, so the system can be solved efficiently using the Cholesky decomposition of P = Λ  Λ ′  , with Λ  ∈    −  and invertible.If this Cholesky decomposition would fail, it means that the problem is ill-posed.

IMPLEMENTATION
Matrix decomposition of numerically rank-deficient matrices.Numerical issues can occur in the symmetric transformation step of the backward substitution (Algorithm 1, Line 7).In this situation the matrix decomposition, as explained in Section 2.3, can cause problems if   is numerically rank-deficient.This can occur even if the constraint Jacobian of the full KKT system has full rank.For clarity, we repeat the decomposition (4), applied to   , at stage  in the backward substitution:  ]  , , (18)   where   represents a matrix consisting of very small values ( , < ).When the decomposition is obtained by use of an SVD factorization,  corresponds to the diagonal matrix consisting of the singular values smaller than the threshold .In the case of an LU factorization with complete pivoting, this type of matrix decomposition can be found by early stopping the factorization when no pivot larger than  can be found.Rather than enforcing the decomposition of the form (17), from a numerical point of view, it is better to continue with the decomposition of the form (18) to avoid using very small pivots.The modifications of the algorithm due to this different decomposition are described below.The top left KKT system (9) changes to: As before, substitute ũ1, = G1,  1 + g1 and , which results in a top left KKT system: Now, as before, eliminate ũ1, , by using the Schur complement of R1 , i.e. ũ1, = − R−1 ) , resulting in a system of the form: Note that the form of this matrix only differs from (11) by the presence of the entry As the values of this entry are very small, it can be neglected in the numerical computations, and the computation continues as before.The modifications, due to this different decomposition, are taken into account in the final backward and forward algorithm (Algorithm 1 and 2, respectively).Iterative refinement.Iterative refinement is a common method for improving the accuracy of a linear system solution 14,15 .The algorithm improves the accuracy of the solution by solving a sequence of linear systems, where the right hand side of the system is the residual of the previous solution.The residual is defined as the difference between the left hand side evaluated at the solution and the right hand side of the system ( =  + ).Since the coefficient matrix of the involved linear systems is unchanged, the factorization of the coefficient matrix from the backward substitution can be reused.Full algorithm.Algorithm 1 outlines the full backward substitution algorithm for arbitrary horizon length , while Algorithm 2 outlines the forward substitution algorithm.Note that some of the matrices appearing in these algorithms are structurally zero, identity or symmetric.In our software implementation this structure is exploited in order to avoid unnecessary floating point operations and memory usage.The quantities without a stage index  are temporary variables, that are not required later in the algorithm.Implementation details.Algorithm 1 and Algorithm 2 are implemented in efficient C ++ -code.The implementation is inspired by the implementation of the classical Riccati recursion in the QP-solver HPIPM 13,16 .As the recursion consists of a series of operations on (small-scale) stagewise matrices that fit in cache for typical problem sizes, we make use of the BLASFEO 17 library, which is performance-optimized for this case.We take into account the structural zero and identity submatrices when performing block matrix operations.For the decompositions of type (4) we use an LU factorization with complete pivoting (pivot treshold is fixed at  = 1 − 5) due to its low computational cost and the presence of structure in   and   , which we exploit in the implementation.

NUMERICAL RESULTS
The performance of the proposed algorithm was benchmarked against three state-of-the-art general-purpose sparse linear solvers for symmetric indefinite systems: MA57 (version 3.11.1) 14, MUMPS (version 5.4.1) 15 and PARDISO (version 6.0) 18 19 20 .Iterative refinement was turned off for all sparse linear solvers.For fairness of comparison, automatic scaling was turned off for MA57, as we found out that it slowed down the solution process significantly.The packages were linked with METIS 21 for constructing a fill-in reducing reordering in the analysis phase and INTEL MKL as BLAS and LAPACK library.For MUMPS we used the sequential version while MA57 and PARDISO were configured to run in a single thread.We compiled MA57 and MUMPS with GFORTRAN, version 9.3.0,with compiler optimization flag -O3.The analysis phase and dynamic work array memory allocation were excluded from the timings.The C/C ++ -code of the implementation of the proposed algorithm and BLASFEO were compiled with GCC, version 9.3.0,with compiler optimization flag -O3.We used X64_INTEL_HASWELL as the target for the BLASFEO library, such that it made use of the AVX2 and FMA Intel® Instruction Set Extensions that were available on the CPU of the test machine.Our test machine was a notebook computer equipped with an Intel® Core™ i7-10850H Processor, running Ubuntu 20.04.The CPU clock frequency was set fixed at 2.7 GHz.
The benchmark set consisted of randomly generated problems and a quadrotor problem.For every problem we evaluated the performance and accuracy of the proposed algorithm and the three sparse linear solvers.To evaluate the performance we used the wall evaluation time and to evaluate the accuracy we used the infinity norm residual ratio.The residual norm ratio is defined as the ratio of the infinity norm of the residual vector to the infinity norm of the right-hand side vector (‖ sol + ‖ ∞ ∕‖‖ ∞ ).

Algorithm 1 Backward Substitution
] ⊳ substitution of dynamics 5: ← number of rows of   7: Calculate  , ,  , ,   and   such that   can be decomposed as in Section 5.
⊳ symmetric transformation 8: [ G, g 15: end for 16: Calculate  , ,  , ,   , such that  0 can be decomposed as in Section 2.3.⊳ factorization of initial stage 17: For the random problems, ten random constrained LQ's were generated for every test dimension, and we took the mean wall time and worst case infinity norm residual ratio as performance and accuracy measure, respectively.

Random problems with initial and terminal constraints
The first experiment consisted of randomly generated problems of fixed control input and state dimensions,   = 5,   = 10.We fully constrained the initial and terminal state with   constraints each.The horizon length  was varied from 10 to 1000.
The results are shown in Figure 1.For all solvers the measured wall times scaled linearly with the horizon length.The proposed algorithm without iterative refinement was roughly eight times faster than MA57, the fastest general-purpose sparse linear solver for these problems.The proposed algorithm with iterative refinement was roughly three to four times faster than MA57.All problems reached an residual norm ratio lower than 10 −9 without iterative refinement and lower than 10 −10 after a single iterative refinement step.The accuracy of the proposed algorithm was comparable to the general-purpose solvers.Iterative refinement was able to improve the accuracy of all problems.
Algorithm 2 Forward substitution

Random problems with constraints on every stage
The second experiment consisted of randomly generated problems of fixed horizon length of  = 100, the state dimension   was varied between 4 and 180, the control input dimensions was   =   ∕2.We put   ∕4 constraints on each state of the control horizon.The results are shown in Figure 2.For large enough problem dimensensions the measured wall times scaled cubically with the state dimension   .The proposed algorithm without iterative refinement was roughly ten times faster than MA57, the fastest general-purpose sparse linear solver for these problems.The proposed algorithm with iterative refinement was roughly four to five times faster than MA57.The numerical accuracy of the proposed algorithm detoriated as problem dimensions increased.For larger state dimensions iterative refinement was not able to consistently improve the accuracy, for some problems iterative refinement resulted in a worse residual norm.The decreasing accuracy for large problem state dimensions is due to the fact that the proposed algorithm does not implement advanced pivoting strategies.Improving in this regard is left for future work.

Quadrotor problem
The state vector of this quadrotor problem consisted of the position, velocity, and three Euler angles, representing the quadrotor's orientation.The control inputs were the acceleration in the upward direction of the drone and the Euler angle rates.The dynamical model was based on the work of Zhang et al. 22 .The initial equality constraint fixed the full state at the reference position and a disturbance in velocity and orientation from equilibrium, while the terminal constraint fixed the full state at the reference position in equilibrium.The quadratic objective encoded the task to stabilize the quadrotor with minimum input energy.The problem dimensions were   = 10,   = 4,  = 50, and 10 initial and terminal constraints.The results are shown in Table 3.The proposed algorithm is roughly twelve times faster than MA57, the fastest solver for this problem.The speed-up reduced to roughly seven times when iterative refinement was applied.The numerical accuracy of the proposed algorithm with iterative refinement was comparable to the accuracy of the other solvers.

DISCUSSION
Comparison with other work.Several connections between the method proposed in this paper and other approaches for solving the constrained LQ problem can be made: • Sideris & Rodriguez 6 developed a factorization algorithm that is able to solve the OCP when the initial state is fixed.
When  , has full row rank the computational complexity is linear with the horizon length, but in general it is cubic.Furthermore, the algorithm assumes positive definiteness of the full-space Hessian.
• Giftthaler & Buchli 8 also make use of the nullspace of   .Whereas we use a parameterization of a basis of this nullspace, their approach makes use of a projection onto this nullspace.This results in a (possibly) singular optimal control problem which can be solved by a modified version of the classical Riccati recursion.The approach has a computational complexity that scales linearly with the horizon length but puts some restrictions on the stagewise constraints, which for example makes the algorithm unable to solve problems where the number of stagewise equalities exceeds the control input dimensions   .In addition, positive semi-definiteness of   and positive definiteness of   are assumed.
• The work of Domahidi et al. 5 is used as the linear solver in the FORCES NLP solver 23 .This solver has proven its use in many application domains such as robotics, automotive and aerospace.The linear solver supports a more general formulation than the constrained LQ problem treated in this paper.It makes use of a structure-exploiting range space method, which is linear with horizon length, but it assumes positive definiteness of the full-space Hessian.
• The method described by Laine & Tomlin 7 also makes use of the nullspace of   , in a similar way as our method.This method however relies on computationally expensive operations that we avoid, such as an SVD for removing linearly dependent constraints.
We did not compare our method to the aforementioned state-of-the-art tailored solvers as they assume regularity constraints that we avoid.Moreover, these papers do not provide publicly available implementations of their algorithms.
Optimal feedback control policies.Apart from solving the LQ system, the quantities appearing in the backward substitution (Algorithm 1) can also be used for obtaining optimal feedback control policies.These policies are useful for developing DDPstyle algorithms.The optimal feedback control policies can be constructed from Algorithm 1, steps 7 to 9 as: with and Here, we recognize   as the feedback gain matrix and   as the feedforward term.This policy automatically satisfies the subset of the constraints that can be satisfied by choosing the appropriate inputs at the current stage, as explained in Section 4. For the first stage the following state constraint has to be satisfied: Limitations of the approach.For large problem state dimensions this resulted in a numerical accuracy that is inferior as compared to the tested general-purpose linear solvers.We believe this is because the algorithm does not implement advanced pivoting strategies.To tackle this problem more advanced pivoting or scaling strategies could be implemented.A difficulty is that the approach allows less freedom in the choice of pivots as compared to general-purpose indefinite solvers.Another possible solution is a different choice of matrix decomposition for the stagewise constraints such as an SVD decomposition instead of the LU decomposition used in this paper.These problems are left for future work.Software implementation.The C ++ implementation of the algorithm proposed in this paper is freely available at https://github.com/lvanroye/generalization_riccati.This repository also contains the code for reproducing the results of this paper.

CONCLUSION AND OUTLOOK
We developed a novel algorithm for efficiently calculating optimal feedback control policies and solutions of stagewise equalityconstrained LQ problems without imposing conservative regularity conditions on the problem at hand.Numerical experiments illustrate that this approach allows for computationally efficient implementations.The promising potential of the proposed approach encourages the authors to develop a nonlinear optimal control problem solver, using the discussed recursion as linear solver.A topic of future work is to investigate more rigorously the impact of inexact arithmetic and improving the numerical accuracy.

Substitution step
Definition 3. A nullspace projection step transforms the KKT matrix where  ∈   ,  ∈ ℝ   × and  ∈ ℝ   × and rank() =   , to a KKT matrix where   is a matrix whose columns span the nullspace of .

Schur complement step
Definition 4. A Schur complement step transforms the KKT matrix where  ∈    and  ∈    and  ∈ ℝ ×  and rank() = , to a KKT matrix ] .
This matrix can only be positive definite if the principal submatrix  ∈    , of   , is positive definite.
Theorem 7. The reduced Hessian of KKT matrix   (29) is positive definite if and only if the reduced Hessian of  is positive definite.
Proof.This result follows directly from Sylvester's law of inertia, Theorem 3, and Theorem 4.

Figure 1
Figure 1 performance and numerical accuracy of proposed algorithm and general-purpose solvers for randomly generated problems in function of horizon length  for problems with   = 10,   = 5 and ten constraints on the initial and terminal state.

Figure 2
Figure 2 performance and numerical accuracy of proposed algorithm and general-purpose linear solvers for randomly generated problems in function of state dimension   for problems with  = 100,   =   ∕2 and   ∕4 constraints on every state of the control horizon.

Theorem 5 .
the substitution steps of the proposed algorithm we have  = [ −   ] .So   = [     − ] ′ .It is easy to verify that substitution of this equation and elimination of the associated dual variables corresponds to the transformation of Definition 3. The reduced Hessian of   (26) is positive definite if and only if the reduced Hessian of the transformed KKT matrix  is positive definite.Proof.A matrix whose columns span the nullspace of the bottom partition of the constraint Jacobian of (26) is   = [      ] .

Theorem 6 .
The reduced Hessian of   (27) has a positive definite reduced Hessian if and only if  and  , the reduced Hessian of the transformed KKT matrix, are positive definite.Proof.Define the invertible matrix  = Sylvester's law of inertia inertia(  ) = inertia() + inertia(  ).

Table 1
summarizes the notation used throughout the paper.

Table 2
Dimensions of submatrices appearing in the algorithmWhen   is nearly rank-deficient, during the factorization a decomposition of the following form can be encountered: +1 ←     +     +

Table 3
performance and numerical accuracy for the quadrotor problem

)
Lemma 1.If the reduced Hessian of   in (27) is positive definite, then  is positive definite.Proof.The constraint Jacobian of   has  = [      ] as a matrix whose columns span its nullspace, where the columns of   span the nullspace of .We can calculate the reduced Hessian   of KKT matrix   as