# Patent

## Title
System and method of accelerated active set search for quadratic programming in real-time model predictive control

## Abstract

An active set algorithm exploits a ‘hot start’ for the set of binding constraints at optimality along with efficient linear algebra to make rapid progress towards the solution. The linear algebra is designed to deal with degenerate constraints as the required factorizations are performed and as degeneracy emerges, and not via a mostly unnecessary pre-process step. Combined together, these novel approaches enable solution of the control problem in real-time.

## Background
This invention was conceived in performance of work under U.S. Government Contract N00421-01-2-0131.


BACKGROUND OF THE INVENTION
The present invention relates to on-board optimization techniques for on-board control, and is particularly useful for Model Predictive Control of a dynamical system.
This invention describes a significant enhancement to existing optimization techniques for on-board control. On-board Model Predictive Control of a nonlinear dynamical system often involves linearizing the nonlinear dynamics and posing a Quadratic Program to stay close to a desired profile of outputs. Since on-board control requires computing a reliable solution in a robust manner in real-time, the quadratic programming algorithm needs to use linear algebra optimally.
Model Predictive Control refers to the procedure of determining optimal operating parameters of a dynamic process based on a model of the ‘plant,’ or the dynamical system. This plant model can be a physics-based model of anything ranging from a gas turbine engine to the acoustics inside a helicopter. It is of interest to engineers to operate such a plant optimally, i.e. meet or exceed certain goals during operation, while honoring physical constraints of the system. To this end, it is common to solve a constrained optimization problem during the operation of the plant, and update the parameters of the optimization problem as the system evolves in time or as the forecast of the future requirements change, and re-solve the problem.
A significant difficulty in this procedure is the need to be able to obtain a reasonable solution for a complex optimization problem in real-time, which is the issue addressed by this invention.
In the past, real-time control has been attempted in chemical engineering applications, particularly in real-time process control. It has also been applied in real-time trajectory planning in aerospace applications. However, the optimization problems arising therein needed to be solved in several minutes, often even hours.
The ‘real-time’ scale allowed by problems of interest here are on the order of milliseconds. The novel algorithm described herein is capable of addressing this time requirement, without sacrificing the fidelity of the solution, and is thus a significant enhancement to existing methods.
Co-pending application, U.S. Ser. No. 10/308,285, filed Dec. 2, 2002, and entitled Real-Time Quadratic Programming for Control of Dynamical Systems, is commonly assigned and one of the inventors is also the inventor of the present application. The problem that invention mainly addresses is the ‘Dynamic Inversion’ control problem spanning over only one time step and with a much smaller budget for computation, and hence a different strategy was used in performing the active set hot start. Further, a different procedure was used for dropping constraints during the search for the active set, which consequently required different linear algebra. Though quite different, the method described in this invention can also be an alternate method for solving the quadratic programming algorithm for Dynamic Inversion (though it may not be the better method for Dynamic Inversion).
SUMMARY OF THE INVENTION
The present invention provides an algorithm and apparatus for controlling a dynamical system in real-time. Such dynamical systems include, but are not limited to, gas turbine engines and aircraft flight control systems. The mechanism of searching for the optimal active set or the set of binding constraints described herein can be applied to any convex optimization problem, including convex quadratic programming.
The present invention provides and exploits an ‘active set hot start,’ i.e. the set of constraints guessed to be binding at optimality. This is different from existing methods which use a starting guess for the solution and not the active set.
The present invention provides a consistent active set method where constraints that are not binding are dropped from the active set, irrespective of the sign of the multiplier. This results in iterates that improve the objective function monotonically.
The present invention also provides an inconsistent active set method where the iterates of the algorithm are not consistent with the active set. This is because most constraints in the initial hot start guess are expected to end up in the optimal active set and hence not dropped, even though inconsistent. While iterates may not improve the objective monotonically, this results in faster convergence and better performance in real-time when the starting guess is even moderately good.
The present invention provides a mechanism for detecting linear dependency in the set of active constraints in an iteration without prior pre-processing using the steps in the current factorization. The present invention also provides a ‘big-K’ formulation of the Quadratic Program for a values of M≧0 and K>0 to simultaneously attain a feasible and optimal solution in the absence of knowledge of an initial feasible point. Any or all of the above techniques can be applied to a sparse version of the optimization.



BRIEF DESCRIPTION OF THE DRAWINGS
Other advantages of the present invention will be readily appreciated as the same becomes better understood by reference to the following detailed description when considered in connection with the accompanying drawings wherein:
The FIGURE illustrates one type of control system that uses the quadratic programming method of the present invention.



DETAILED DESCRIPTION OF A PREFERRED EMBODIMENT
The Generic Problem
FIG. 1 is a generic model of a control system 10 using Model Predictive Control and of a type that would benefit from the present invention. The control system 10 includes a desired trajectory generator 12 which creates a desired profile of the outputs of the system 10. A linearization module 14 derives a linearized model about the desired trajectory from the desired trajectory generator 12. A quadratic Programming formulation module 16 forms a quadratic program to determine a control profile for best attaining the desired trajectory while respecting any constraints. The Quadratic Programming Solver 18 solves the optimization problem established by the formulation module 16 to generate a profile of the optimal controls. The Quadratic Programming Solver 18 is the focus of this invention. The profile of the optimal controls is sent to an actuation system 20, which acts on the plant 22 of the system 10. The sensor system 24 provides feedback from the plant 22 to the desired trajectory generator 12.
The forthcoming formulation of the optimization problem for the MPC is included here only for the sake of completeness. Consider a nonlinear dynamical system with control variables u, state variables x and responses (or outputs) y, that are related as




  
    
      d
      x
    
    
      d
      t
    
  
  =
  
    φ
    ⁡
    
      (
      
        x
        ,
        u
      
      )
    
  




y=h(x,u)

A discrete time version of the above with uniform time intervals can be written as

xt+1=xt+φ(xt,ut)Δt

yt=h(xt,ut) t=1, 2, . . . , N.

The nonlinear functions φ and h are commonly linearized about base points which are steady state points, i.e., ones at which x vanishes. Given such a steady state base point xs,us, i.e., one where φ(xs,us)=0, the discrete time system can be linearized as

xt+1=xt+φx(xs,us)(xt−xs)Δt+φu(xs,us)(ut−us)Δt

yt=h(xs,us)+hx(xs,us)(xt−xx)Δt+hu(xs,us)(ut−us)Δt

t=1,2, . . . , N,

where φx denotes the Jacobian of φ with respect to x, and so on.

Control engineers commonly express the above system as

xt+1=xbt+Atxt+Btut  (1)

yt=ybt+Ctxt+Dtut, t=1,2, . . . , N.  (2)

where

At=I+φx(xs,us)Δt

Bt=φu(xs,us)Δt

Ct=hx(xs,us)Δt

Dt=hu(xs,us)Δt

xbt=−(φx(xs,us)xs+φu(xs,us)us)Δt

ybt=h(xs,us)−(hx(xs,us)xs+hu(xs,us)us)Δt.

The time dependence of the above parameters that define the linearized discrete time system is tacitly bidden in (xs,us), the point about which the linearization is performed, which is chosen afresh for each time point. Note that in this quasi-Linear parameter (q-LPV) varying model, this point of linearization can be a convex combination of several steady state points. The q-LPV model, while no substitute for a true nonlinear model, is often sufficient for the level of accuracy required in a feedback control framework. If a true nonlinear model were to become available, the techniques described herein are just as applicable to the quadratic programs obtained from a true linearization.
Objective Given the above description of the system, the aim is to optimally control the system over a particular time window. The degree of success in doing so is measured by how closely the variables of the system track certain reference trajectories. If Tty,Ttx,Ttu represent reference trajectories for yt,xt,ut, the desired objective can be expressed as




  
    
      min
      
        x
        ,
        y
        ,
        u
      
    
    ⁢
    
      
        ∑
        t
        
          
        
      
      ⁢
      
        
      
      ⁢
      
        
          
            (
            
              
                y
                t
              
              -
              
                T
                t
                y
              
            
            )
          
          T
        
        ⁢
        
          
            W
            y
          
          ⁡
          
            (
            
              
                y
                t
              
              -
              
                T
                t
                y
              
            
            )
          
        
      
    
  
  +
  
    
      ∑
      t
      
        
      
    
    ⁢
    
      
    
    ⁢
    
      
        
          (
          
            
              x
              t
            
            -
            
              T
              t
              x
            
          
          )
        
        T
      
      ⁢
      
        
          W
          x
        
        ⁡
        
          (
          
            
              x
              t
            
            -
            
              T
              t
              x
            
          
          )
        
      
    
  
  +
  
    
      ∑
      t
      
        
      
    
    ⁢
    
      
    
    ⁢
    
      
        
          (
          
            
              u
              t
            
            -
            
              T
              t
              u
            
          
          )
        
        T
      
      ⁢
      
        
          W
          u
        
        ⁡
        
          (
          
            
              u
              t
            
            -
            
              T
              t
              u
            
          
          )
        
      
    
  




The diagonals of the weighting matrices represent the relative weights of the various objectives. We usually have only one or two primary objectives, while the rest are secondary. While the weights on the secondary objective are set to a small number, they are always non-zero and sufficiently large so that the Hessian of the problem is not very poorly conditioned. We generally try to keep the condition number below 107 for double precision computation. This is also necessary to ensure that there is a unique solution for the optimal controls even in the absence of constraints. If not, imagine the case where the system could be in steady state, but yet the optimal controls could be jumping around, causing what engineers call ‘chattering.’ The reason for re-casting the problem formulation is to justify solving a convex quadratic program, and not to cover the formulation under this patent.
Constraints Bound constraints are typically imposed on x,y,u, as well as on their rates of change. In addition, there are other linear inequality constraints involving combinations of these variables. The inequality constraints can thus be expressed as

A0U≦b0

where





  U
  =
  
    (
    
      
        
          
            y
            1
          
        
      
      
        
          
            u
            1
          
        
      
      
        
          
            x
            2
          
        
      
      
        
          
            y
            2
          
        
      
      
        
          
            u
            2
          
        
      
      
        
          ⋮
        
      
      
        
          
            x
            N
          
        
      
      
        
          
            y
            N
          
        
      
      
        
          
            u
            N
          
        
      
    
    )
  




represents the vector of optimization variables. Note that x1 is not an optimization variable since the initial state and control variables from the prior time determine the states x1 at the first time point.
The Quadratic Program
We can represent the above as a strictly convex Quadratic Program (QP) in the following form




  
    
      
        
          
            
              min
              p
            
            ⁢
            
              
                1
                2
              
              ⁢
              
                p
                T
              
              ⁢
              
                Z
                T
              
              ⁢
              
                H
                0
              
              ⁢
              Zp
            
          
          +
          
            
              
                (
                
                  
                    c
                    0
                  
                  +
                  
                    
                      H
                      0
                    
                    ⁢
                    
                      U
                      f
                    
                  
                
                )
              
              T
            
            ⁢
            Zp
          
        
        ⁢
        
          

        
        ⁢
        
          
            
              s
              .
              t
              .
              
                
              
              ⁢
              
                A
                0
              
            
            ⁢
            Zp
          
          ≤
          
            
              b
              0
            
            -
            
              
                A
                0
              
              ⁢
              
                
                  U
                  f
                
                .
              
            
          
        
      
    
    
      
        (
        3
        )
      
    
  




The above QP with equality and inequality constraints is large and sparse. Experience with active set algorithms for real-time control has shown the advantage of considering a dense formulation with fewer variables. Below we outline a way of deriving the above dense formulation for the original QP.
Let Z be a matrix whose columns form a basis for the null space of E0. Given any particular Uf that satisfies EuUf=r0, the family of solutions for the equality constraints can be expressed as

U=Uf+Zp  (4)

where pεm, and m is the dimension of the null space of E0.

Substituting (4) in the original problem yields the following reduced dense QP in p:




  
    
      
        
          
            
              min
              p
            
            ⁢
            
              
                1
                2
              
              ⁢
              
                p
                T
              
              ⁢
              
                Z
                T
              
              ⁢
              
                H
                0
              
              ⁢
              Zp
            
          
          +
          
            
              
                (
                
                  
                    c
                    0
                  
                  +
                  
                    
                      H
                      0
                    
                    ⁢
                    
                      U
                      f
                    
                  
                
                )
              
              T
            
            ⁢
            Zp
          
        
        ⁢
        
          

        
        ⁢
        
          
            
              s
              .
              t
              .
              
                
              
              ⁢
              
                A
                0
              
            
            ⁢
            Zp
          
          ≤
          
            
              b
              0
            
            -
            
              
                A
                0
              
              ⁢
              
                U
                f
              
            
          
        
      
    
    
      
        (
        5
        )
      
    
  




The next section describes a method for computing the matrix Z by exploiting the state equations (1) and (2) defining the equality constraints in (3), and without performing an actual factorization.
Computing a Basis for the Null Space
The null space of interest comprises all U that satisfy

E0U=0.

Consider splitting the variables U into UB and UNB. Let the columns of E0 corresponding to UB form the submatrix EB and those corresponding to UNB form ENB. We will assume that the splitting is done in such a manner that EB is square and non-singular. Then for all U in the null space,

EBUB+ENBUNB=0

    
    
        i.e. UB=−EB−1ENBUNB.
    
    


Thus all vectors U in the null space of E0 can be written as




  U
  =
  
    
      (
      
        
          
            
              U
              B
            
          
        
        
          
            
              U
              NB
            
          
        
      
      )
    
    =
    
      
        (
        
          
            
              
                
                  -
                  
                    E
                    B
                    
                      -
                      1
                    
                  
                
                ⁢
                
                  E
                  NB
                
                ⁢
                
                  U
                  NB
                
              
            
          
          
            
              
                U
                NB
              
            
          
        
        )
      
      =
      
        
          (
          
            
              
                
                  
                    -
                    
                      E
                      B
                      
                        -
                        1
                      
                    
                  
                  ⁢
                  
                    E
                    NB
                  
                
              
            
            
              
                I
              
            
          
          )
        
        ⁢
        
          
            U
            NB
          
          .
        
      
    
  




It is not difficult to argue that the matrix




  Z
  =
  
    (
    
      
        
          
            
              -
              
                E
                B
                
                  -
                  1
                
              
            
            ⁢
            
              E
              NB
            
          
        
      
      
        
          I
        
      
    
    )
  




contains columns that form a basis for the null space of interest, and the vector UNB assumes the role of p. In contrast with, say, a null space computed using a QR factorization, part of it is a sparse identity block. Even more importantly, it is never necessary to invert the matrix EB in order to compute the rest of Z if we choose our variable splitting wisely, as shown next.
Let us choose UNB as the vector of control variables stacked in time, i.e.,




  
    U
    NB
  
  =
  
    u
    =
    
      
        (
        
          
            
              
                u
                1
              
            
          
          
            
              
                u
                2
              
            
          
          
            
              ⋮
            
          
          
            
              
                u
                N
              
            
          
        
        )
      
      .
    
  




UB then becomes the corresponding vector of outputs and state variables, stacked in time. By examining UB=−EB−1ENUNB, it is evident that the ith column of −EB−1ENB is given by the value of UB when UNB=ei, the vector with a 1 in its ith position, and zeros elsewhere. This reduces to computing the values of the states and outputs for every setting ei of UNB, which, amounts to running a simulation through the state equations (1) and (2) by setting a control variable in one particular time point to 1, and the rest to 0. Thus no actual factorization is needed.
This is essentially equivalent to computing what is known in control theory as the standard basis. However, we just derived it from a separate mathematical viewpoint.
The Reduced, Dense Problem
Based on the choice of Z and p=UNB=u in the previous subsection, the QP in (5) assumes the form




  
    
      
        
          
            
              min
              u
            
            ⁢
            
              
                1
                2
              
              ⁢
              
                u
                T
              
              ⁢
              Hu
            
          
          +
          
            
              c
              T
            
            ⁢
            u
          
        
        ⁢
        
          

        
        ⁢
        
          
            
              s
              .
              t
              .
              
                
              
              ⁢
              Au
            
            ≤
            b
          
          ,
        
      
    
    
      
        (
        6
        )
      
    
  




where

H=ZTH0Z, c=ZT(c0+H0Uf),

A=A0Z, b=b0−A0Uf.

It is this reduced dense strictly convex QP with inequality constraints that we will develop algorithms for in the rest of the paper. Note that the iterations and fundamental math in the QP algorithm are equally applicable to sparse problems as well.
Algorithm for Searching for the Optimal Active Set
Active set algorithms search iteratively for the set of binding constraints at optimality, usually referred to as the active set. For model-predictive control problems, the solution to the MPC problem in the current time step provides a guess for the optimal active set in the next time step. This particular guess is used to great advantage to cut down on the number of iterations required for solving the QP to convergence, a feature that is practically indispensable in real-time control. The broad steps of the active set algorithm are sketched below, and the associated linear algebra is elaborated in the following section.

    
    
        Start off with a guess for an active set. We will denote the set of indices of constraints in the active set by W, and also characterize by E the rows in the constraint matrix A corresponding to the guessed active set. Assume a feasible point uf is known, i.e., Auf≦b.
        In iteration k, given a guess E for the active constraints, solve the Equality-Constrained QP (EQP)
    
    






  
    
      
        
          
            
              min
              u
            
            ⁢
            
              
                1
                2
              
              ⁢
              
                u
                T
              
              ⁢
              Hu
            
          
          +
          
            
              c
              T
            
            ⁢
            u
          
        
        ⁢
        
          

        
        ⁢
        
          
            
              s
              .
              t
              .
              
                
              
              ⁢
              Eu
            
            =
            r
          
          ,
        
      
    
    
      
        (
        7
        )
      
    
  




    
    
         where r represents the sub-vector of right hand sides for the active constraints. The optimal u*, and Lagrange multipliers λ* are given by
    
    






  
    
      
        
          
            [
            
              
                
                  H
                
                
                  
                    E
                    T
                  
                
              
              
                
                  E
                
                
                  0
                
              
            
            ]
          
          ⁡
          
            [
            
              
                
                  
                    u
                    *
                  
                
              
              
                
                  
                    λ
                    *
                  
                
              
            
            ]
          
        
        =
        
          [
          
            
              
                
                  -
                  c
                
              
            
            
              
                r
              
            
          
          ]
        
      
    
    
      
        (
        8
        )
      
    
  




Ratio Test Determine largest as αε[0,1] such that uk−1+αs is feasible, where s=u*−uk−1, and uk−1 denotes the prior iterate. Note that u0=uf. In other words, α is picked to be the largest value in [0,1] such that Auk−1+αAs≦b. This test need not be performed for constraint indices i that are in the active set, since they have As=0 (or Asi≦0 in iteration 1), and the prior iterate uk−1 is feasible. (footnote: It is possible to have degenerate constraints not in the active set that have Asi=0, and the ratio test over such constraints would also be skipped.) Thus α is given by




  α
  =
  
    
      min
      
        i
        ∉
        W
      
    
    ⁢
    
      
        
          
            b
            i
          
          -
          
            
              (
              
                Au
                
                  k
                  -
                  1
                
              
              )
            
            ⁢
            i
          
        
        
          As
          i
        
      
      .
    
  




    
    
        Update the guess for the active set in the next iterate.
        
            Add to W the first index iB corresponding to which α achieves its minimum value in the ratio test, the guess for the active indices in the next iterate. The corresponding constraint is loosely referred to as the ‘tightest constraint’.
            If there exist constraints for which the Lagrange multiplier λ* in the EQP (7) is negative, drop the constraint with the most negative multiplier from the active set. This is the so-called steepest edge rule. Bland's Rule is deployed whenever degeneracy is detected in the active set, and in these instances, the first constraint with a negative multiplier is dropped. Details on treatment and detection of degeneracy appear later in this disclosure.
            Special case for first iteration Define a constraint in the initial active set to be consistent if the constraint is binding at uf. If in the first iteration α<1, then all constraints that are not consistent are dropped, irrespective of the sign of their multiplier. This is because if Euf≦r with strict inequality for some components, the updated iterate u1=uf+α(u*−uf) does not satisfy Eu1=r for all constraints in the active set for the next iteration, leading to a discrepancy. This is the consistent active set method. For example, if uf is in the strict interior of the feasible set, this could lead to dropping all the constraints and re-starting with an empty active set, thereby allowing little exploitation of ‘hot start’. However, if uf is the set of optimal controls obtained by solving the MPC problem at the prior time step, it is usually consistent with the initial guess for the active set.
            The Inconsistent active set method In some cases, the known feasible point is largely inconsistent with the starting guess for the active set. This leads to many inconsistent constraints being dropped from the active set at the end of the first iteration. If the initial hot start guess is a good one, many iterations are wasted in picking these constraints back up. Thus another approach is to retain the inconsistent constraints in the active set and continue the search for the optimum even though the iterate and the active set disagree. This inconsistency disappears if in any iteration α=1. Moreover, the degree of inconsistency reduces at every iteration. The inconsistent method usually allows far more rapid convergence to the optimum than the consistent method.
        
        
    
    


Update the iterate

uk=uk−1+αs.

Convergence to the global optimum is achieved when α=1 and λ*≧0, i.e., when the active set does not change.
If the allowed time within the real-time interval is exhausted before convergence is reached, the last updated iterate is returned as the solution.
Monotonically improving iterates It can be proved by using convexity that the iterates in the consistent active set algorithm improve the quadratic objective monotonically (with strict improvement for non-zero α).
Details of Linear Algebra
In this section, we are going to describe linear algebra computations necessary to protect us in the event of degeneracy and to obtain an exact solution to the EQP with as little computation as possible.
Since in our problem H is strictly positive definite, u can be eliminated in (8) and the solution EQP (7) can be written in closed form as

λ*=−(EH−1ET)−1(r+EH−1c)  (9)

u*=−H−1c−(EH−1)Tλ*  (10)

Since we can perform a Cholesky factorization of H (H=LLT, L is lower triangular) at the onset and re-use it at every iteration, this is cheaper than solving the larger system (8) at every iteration. Moreover, it should be noted that the unconstrained solution to the quadratic objective,

uunc=−H−1c

can also be computed at the onset using the Cholesky factorization and re-used at every iteration in the computations in (9) and (10).
Degeneracy
Degeneracy is caused by introducing redundant constraints whose physical equivalents are already represented by other constraints, and which could alternately have been expressed as linear combinations of other constraints. Often, such dependent constraints could become active at the same time, and end up in the working set. This results in the loss of row rank in E, the matrix of active constraints, leading to a zero or near-zero condition number reciprocal for EH−1ET, the coefficient matrix in computing λ* as per (9).
The suggested approach in standard practice/literature is to make sure that redundant constraints do not appear in the problem formulation. This may not be possible because we have to allow for the possibility that constraints may become degenerate ‘on the fly.’ This is more so, given that often near-redundant constraints based on multiple sensor readings are imposed to ensure robustness with respect to not violating the constraint.
The second safeguard adopted in most commercial codes is that of pre-processing, or eliminating redundant constraints by a prior examination of the constraint matrix. This involves steps similar to Gaussian Elimination, and is extra computation wasted in the dominant majority of cases where degeneracy is not an issue.
In contrast, we aim to detect and deal with degeneracy in the active set as it shows up, and without doing any extra linear algebra than what we would need to solve the EQP anyway. In the invention Accelerated Quadratic Programming for On-board Real-Time Control, this was achieved by performing a QR factorization of EH−1ET in determining (9), and setting the components of λ* corresponding to near-zero pivots in R to zero. This is consistent with the fact that the redundant constraints do not play a role in the optimization.
Here we take advantage of the fact that the active set changes more traditionally, i.e. we pick up one binding constraint, and drop no more than one constraint, as opposed to drop all constraints with negative multipliers. The computations sketched out below allow us to efficiently update the product EH−1ET as well as the associated factorization, while detecting degeneracy in course of the factorization.
Given L, the Cholesky factor of H,

EH−1ET=MTM, where M=L−1ET.

Instead of performing a QR factorization of MTM, we perform a QR factorization of M itself. The advantage is that this, much like the other alternative of QR on MTM, reveals any rank loss in E due to degeneracy, while performing the factorization on M which has a condition number that is only the square root of the condition number of MTM. Consider computing a sequence of Householder transformations Qi needed for the QR decomposition of M:

Q(m−1) . . . Q(2)Q(1)M=R.

Let na be the number of active constraints, i.e., the number of rows in E or M. The jth Householder transformation operating on column j of M zeros out every element in column j, rows j+1 through na. Thus, the upper triangular matrix R is formed. Let v be the column vector such that the first j−1 elements are 0, and on the rest, matches the elements of column j of Q(j−1) . . . Q(1)M (the intermediate reduced matrix). This Householder transformation can then be expressed as




  
    Q
    
      (
      j
      )
    
  
  =
  
    I
    -
    
      
        
          2
          ⁢
          
            
          
          ⁢
          υ
          ⁢
          
            
          
          ⁢
          
            υ
            T
          
        
        
          
            υ
            T
          
          ⁢
          υ
        
      
      .
    
  




If column j of E represents a degenerate constraint, and can be formed as a linear combination of columns 1 through j−1 of E, the same is true of column j of M. Then the preceding Householder transformations Q(j−1) through Q(1) on M would have already zeroed out elements j through na in column j of the intermediate reduced matrix. Then the vector v for the Householder transformation based on that sub-column would be all zeros, thereby indicating the presence of a linearly dependent constraint. Whenever we detect such degeneracy, we skip the corresponding column of M and set the corresponding λ*j to 0, effectively dropping the constraint as degenerate. We then find the next column that offers a non-zero v and compute the corresponding Householder transformation. Since the product of the resulting Householder transformations is orthogonal, the resulting R satisfies

RTR={circumflex over (M)}T{circumflex over (M)}=ÊH−1ÊT,

Where Ê is the original matrix E of active constraints with the degenerate rows excluded. Thus detection of degeneracy and factorization are achieved simultaneously.
Let Ĝ=L−T{circumflex over (M)}=H−1ÊT. Now λ* can be computed by a forward solve and a back solve, i.e., two O(n2) operations on the system

RTRλ*=−r−ĜTc.  (11)

We can compute u* as

u*=uunc−ĜTc.

Updating Matrix Products
The matrices M and G=L−TM that appear in the preceding computations can be updated given the matrices from the prior iteration, rather than computed from scratch, which reduces an expensive matrix product operation to two O(n2) operations.
Consider the matrix {tilde over (E)} that has an extra row, a, added to E. Without loss of generality, this can be expressed as




  
    E
    ~
  
  =
  
    
      [
      
        
          
            E
          
        
        
          
            a
          
        
      
      ]
    
    .
  




Then

{tilde over (M)}=L−1{tilde over (E)}T=[L−1ET|L−1aT]=[M|L−1aT].

Given the knowledge of M, all that is required to be computed is L−1aT, which is a forward sole on the system Lξ=aT costing only O(n2). Similarly

{tilde over (G)}=L−1{tilde over (M)}=[L−TM|L−TL−1aT]=[G|L−Tξ],

and can thus be updated with a similar O(n2) operation.
In going from one active set iteration to the next, either a row is dropped from E (update is simply to drop the corresponding column from M and G), or a row is added, or both, and it is clear how the matrix products can be updated in all of these cases. Moreover, in our implementation, we store all the vectors ξ and L−Tξ from prior iterations, so if a constraint that was ever active re-appears in the active set at a later iteration, the corresponding ξ and L−Tξ do not need to be re-computed. This requires extra storage equal to twice the size of A, which is of minimal concern given our reduced dense formulation of the MPC problem and available memory.
Updating the Factorization
It is also possible to update the QR factorization of M at every iteration, which essentially replaces the only remaining O(n3) operations with cheaper computation.
Consider the sequence of Householder transformations that put M in upper triangular form

QTM=R.

Consider first the simple case where only one row a has been added to E (and hence M), and none dropped. Then

QT{tilde over (M)}=QT[M|L−1aT]=[QTM|QTL−1aT]=[R|QTξ]

Appending QTξ to R, which has more rows than columns and is upper triangular, results in a structure similar to




  
    
      
        Q
        T
      
      ⁢
      
        M
        ~
      
    
    =
    
      [
      
        
          
            *
          
          
            *
          
          
            *
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            *
          
          
            *
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            *
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            ×
          
        
        
          
            ·
          
          
            ·
          
          
            ·
          
          
            ·
          
          
            ·
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            ×
          
        
      
      ]
    
  
  ,




where * denotes elements of R and x denotes elements of the vector QTξ.
It is easy to see that one Householder transformation, say {tilde over (Q)}, on the last column can put the matrix in upper triangular form, i.e.




  
    
      
        Q
        ~
      
      ⁢
      
        
      
      ⁢
      
        Q
        T
      
      ⁢
      
        M
        ~
      
    
    =
    
      [
      
        
          
            *
          
          
            *
          
          
            *
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            *
          
          
            *
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            *
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            *
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
        
        
          
            ·
          
          
            ·
          
          
            ·
          
          
            ·
          
          
            ·
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            0
          
        
      
      ]
    
  
  ,




which costs only O(n), involving a simple norm computation of the relevant sub-vector in the last column. This updates the R matrix, and the updated Q can be represented as {tilde over (Q)}QT.

The update is slightly more complicated when a row is dropped from E. Equivalency, consider M obtained by dropping an intermediate column, and suppose QT represents the sequenced of Householder transformations that put M in triangular form. Then




  
    
      
        Q
        T
      
      ⁢
      
        M
        ~
      
    
    =
    
      [
      
        
          
            *
          
          
            *
          
          
            *
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            *
          
          
            *
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            *
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            
              *
              ^
            
          
          
            *
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            
              *
              ^
            
          
          
            ×
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            
              *
              ^
            
          
        
        
          
            ·
          
          
            ·
          
          
            ·
          
          
            ·
          
          
            ·
          
        
        
          
            0
          
          
            0
          
          
            0
          
          
            0
          
          
            ×
          
        
      
      ]
    
  
  ,




where {circumflex over (*)} represents the elements previously on the diagonal of R that have spilled into the first lower triangular band due to the dropping of the intermediate column. If the jth column was dropped, a sequence of Given Rotations can be applied to QT{tilde over (M)} to zero out the lower triangular band. This involves at most the cost of two Householder transformations, depending on the location of the dropped column. Let such a sequence of Givens rotation be Q1, . . . , Qmj, i.e.,





  
    
      Q
      
        m
        j
      
    
    ⁢
    
      
    
    ⁢
    ...
    ⁢
    
      
    
    ⁢
    
      
        Q
        _
      
      1
    
    ⁢
    
      Q
      T
    
    ⁢
    
      M
      ~
    
  
  =
  
    [
    
      
        
          *
        
        
          *
        
        
          *
        
        
          *
        
        
          *
        
      
      
        
          0
        
        
          *
        
        
          *
        
        
          *
        
        
          *
        
      
      
        
          0
        
        
          0
        
        
          *
        
        
          *
        
        
          *
        
      
      
        
          0
        
        
          0
        
        
          0
        
        
          *
        
        
          *
        
      
      
        
          0
        
        
          0
        
        
          0
        
        
          0
        
        
          *
        
      
      
        
          0
        
        
          0
        
        
          0
        
        
          0
        
        
          0
        
      
      
        
          ·
        
        
          ·
        
        
          ·
        
        
          ·
        
        
          ·
        
      
      
        
          0
        
        
          0
        
        
          0
        
        
          0
        
        
          0
        
      
    
    ]
  




The upper-triangularized matrix above thus provides the updated R, while the updated Q can be expressed as QQ1T . . . QmjT.
Observe that it is never necessary to store any of the Householder or Givens Rotation matrices. One can simply store the vectors v associated with the Householder transformations, and the parameter cos θ associated with each Givens transformation in the computation. This is because to perform a multiplication with a column, it is cheaper to multiply by the sequence of orthogonal transformations using the properties of the transformation than it is to form the product of the orthogonal transformations and multiply by the resulting dense matrix.
Restoring Feasibility
Since the problem we are solving has nonlinear dynamics, it is often the case that owing to differences caused by a new linearization, the optimal controls uf from the previous time period are not feasible in the current time period. Such infeasibility is also incurred by Betts in the course of solving the QPs arising in his SQP approach to the nonlinear MPC, and he suggests solving a ‘phase-I’ problem to minimize infeasibility, a common approach in problems with linear constraints. However, with this approach, we could use up the allowed computation time and still not have a feasible solution, while having made no progress towards optimality. For these reasons, we choose to solve the so-called ‘big-M’ problem, which minimizes a weighted sum of the original objective and a measure of infeasibility. Instead of solving the original QP (6), the traditional big-M method introduces an extra variables τ and proposes to solve




  
    
      min
      
        u
        ,
        τ
      
    
    ⁢
    
      
        1
        2
      
      ⁢
      
        u
        T
      
      ⁢
      
        
      
      ⁢
      Hu
    
  
  +
  
    
      c
      T
    
    ⁢
    
      
    
    ⁢
    u
  
  +
  
    M
    ⁢
    
      
    
    ⁢
    τ
  




s.t. Au≦b+τd,

τ≧0

where d is a vector with a 1 in position I if aiuf>bi (ai represents the ith row of A), zeros elsewhere. It can be observed that u=uf,τ=maxi{aiuf−bi} is a feasible point for the big M QP, so we have knowledge of a feasible point.
It can be deduced from the theory of exact penalty functions that there exists a finite M_ such that for all M≧M_, the solution to the big M QP is feasible and optimal for the original QP (6), i.e., has τ=0.
In order to adapt this method for our problem, we need the Hessian quantity with respect to u and τ to be invertible. To achieve this, we add a term 1⁄2Kτ2 to the objective. Numerical experiments further led us to set M=0, thereby alleviating the need of the τ≧0 constraint, though it is possible to leave a non-zero but positive M and the constraint τ≧0 in. Our problem, which we will call the ‘big-K’ problem, thus looks like




  
    
      min
      
        u
        ,
        τ
      
    
    ⁢
    
      
        1
        2
      
      ⁢
      
        u
        T
      
      ⁢
      
        
      
      ⁢
      Hu
    
  
  +
  
    
      c
      T
    
    ⁢
    u
  
  +
  
    
      1
      2
    
    ⁢
    K
    ⁢
    
      
    
    ⁢
    
      τ
      2
    
  




s.t. Au≦b+τd.

It is easy to see that the result implied by the theory of exact penalty functions for the big M problem still holds.
Guessing the Initial Active Set
The guess for the initial active set is intimately related to the model predictive control problem formulation. Let

T={t0,t0+Δt,t0+2Δt, . . . t0+(N−1)Δt}

denote the set of time points under consideration in the time window of the particular MPC problem at time t0. The variables associated with the dynamical system can be expressed as x(T), u(T), y(T) where, for example, u(T) denotes the set of control variables at the time points under consideration, i.e.,

u(T)={u(t0),u(t0+Δt),u(t0+2Δt), . . . u(t0+(N−1)Δt).}

Further, in a slightly different notation, (u still denotes the optimization variable) we can describe any particular time-dependent constraint g of th MPC problem as

0≦gt(u)≦βt, for all tεT.

Note that since tεT, each such constraint actually appears as N algebraic constraints in the optimization problem. For our quadratic program, g is actually linear, and gt is a function of no more than ut−1, ut, tt+1. However, the method of arriving at an initial guess for the active set is applicable in the much more general setting.
Suppose u*t0 denotes the values of the optimal controls arrived at by executing the iterative algorithm on the optimization problem corresponding to the MPC problem at time t0. The final iteration of the active set algorithm for this problem tells us which constraints are in the active set. Suppose that gt is active for tεT_⊂T (T_ could represent some or all of the points in the set T), i.e.,

gt(u*t0)=βt for all tεT−.

Then the MPC problem at the next time point t0+Δt is defined over the shifted time window

T′={t0+Δt,t0+2Δt, . . . t0+NΔt}

Correspondingly, the constraint gt appears as

0≦gt(u)≦βt for all tεT′,

in this following MPC problem. This is the optimization problem we want to active set guess for.
Then, as an initial guess for the MPC problem at time t0+Δt, the constraint gt is guessed to be active for all tεT_∩T′ (and hence put in the initial active set W0). Further, gt is guessed to be active at the last time point t0+NΔt, of the MPC problem at time t0+Δt if it was also active in the last time point t0+(N−1)Δt of the MPC problem at time t0.
This procedure is repeated for all the constraints in the MPC problem to arrive at the initial guess for the active set.
In accordance with the provisions of the patent statutes and jurisprudence, exemplary configurations described above are considered to represent a preferred embodiment of the invention. However, it should be noted that the invention can be practiced otherwise than as specifically illustrated and described without departing from its spirit or scope. Alphanumeric identifiers for steps in the method claims are for ease of reference by dependent claims, and do not indicate a required sequence, unless otherwise indicated.

## Claims

1. A method for controlling a multivariable system including the steps of:
a) sensing current conditions of the system and generating a plurality of sensor signals indicating the current conditions of the system;
b) receiving a plurality of commands;
c) in each of a plurality of time steps, formulating a problem of achieving a desired search for the best active set based on a final active set of a prior time step of the dynamic response for a window spanning multiple time steps as a solution to a quadratic programming problem based upon the commands and the sensor signals;
d) solving the quadratic programming problem in each time step using an interative algorithm which searches for an optional active set;
e) in each subsequent time step of the plurality of time steps, initializing a search for the best active set based on a final active set of a prior time set of the plurality of time steps; and;
f) effecting the desired dynamic response on the system based upon said step d).

2. The method of claim 1 wherein the active set comprises a set of constraints that are binding at an optimized solution.

3. The method of claim 2 wherein said step e) further includes the procedure of dropping only one constraint with a negative Lagrange multiplier.

4. The method of claim 2 wherein said step e) further includes the procedure of dropping all constraints that are not binding irrespective of signs of Lagrange multipliers of the constraints.

5. The method of claim 2 further including the steps of:
determining that the final active set of the prior time step is not feasible in a current timestep;
minimizing a weighted sum of the original QP problem and a measure of infeasibility.

6. The method of claim 2 further including the steps of:
determining that the feasible point is largely inconsistent with the active set; and
retaining inconsistent constraints in the active set while continuing the optimization.

7. The method of claim 1 further including the step of performing a QR factorization in each iterate of the iterative algorithm of said step e).

8. The method of claim 7 wherein a first iterate of the iterative algorithm includes the step of performing a full QR factorization and subsequent iterates include the step of performing a QR factorization update.

9. The method of claim 7 further including the step of detecting degeneracy during each QR factorization.

10. The method of claim 9 further including the step of dropping a degenerate constraint.

11. The method of claim 10 further including the step of detecting linear dependency in the active constraints in each of the iterations without pre-processing.

12. A method for controlling a multivariable system including the steps of:
a) sensing current conditions of the system and generating a plurality of sensor signals indicating the current conditions of the system;
b) receiving a plurality of commands;
c) in each of a plurality of time steps, formulating a problem of achieving a desired dynamic response for a window spanning multiple time steps as a solution to a quadratic programming problem based upon the commands and the sensor signals;
d) iteratively solving the quadratic programming problem each time step by:
performing a Cholesky factorization of H in a first iteration;
reusing the Cholesky factorization in iterations subsequent to the first iteration;
computing an unconstrained solution in one of the iterations; and
reusing the unconstrained solution in iterations subsequent to the one of the iterations.
e) generating commands based upon the solutions to the quadratic programming problem and sending the commands to actuators to generate a physical input to the system.

13. The method of claim 12 further including the step of performing a QR factorization in each iterate of the iterative algorithm.

14. The method of claim 13 wherein an initial iterate of the iterative algorithm includes the step of performing a full QR factorization and subsequent iterates include the step of performing a QR factorization update.

15. The method of claim 13 further including the step of detecting degeneracy during each QR factorization.

16. The method of claim 15 further including the step of dropping a degenerate constraint.

17. The method of claim 16 further including the step of detecting linear dependency in the active constraints in each of the iterations without pre-processing.
e) generating commands based upon the solutions to the quadratic programming problem.

18. A method for controlling a multivariable system including the steps of:
a) sensing current conditions of the system and generating a plurality of sensor signals indicating the current conditions of the system;
b) receiving a plurality of commands;
c) in each of a plurality of time steps, formulating a problem of achieving a desired dynamic response for a window spanning multiple time steps as a solution to a quadratic programming problem based upon the commands and the sensor signals;
d) iteratively solving the quadratic programming problem where a set of indices of constraints in the active set is denoted by W and the rows in the constraint matrix A corresponding to the active set are characterized by E, and wherein a feasible point uf is known, Auf≦b;
e) in said step d), choosing an initial guess active set;
f) in said step d), in iteration k, given a guess E for the active constraints, solving the Equality-Constrained QP (EQP)





  
    
      min
      u
    
    ⁢
    
      
        1
        2
      
      ⁢
      
        u
        T
      
      ⁢
      Hu
    
  
  +
  
    
      c
      T
    
    ⁢
    u
  



s.t. Eu=r,
where r represents the sub-vector of right hand sides for the active constraints;
g) in said step d), determining the optimal u*, and Lagrange multipliers λ* as





  
    
      [
      
        
          
            H
          
          
            
              E
              T
            
          
        
        
          
            E
          
          
            0
          
        
      
      ]
    
    ⁡
    
      [
      
        
          
            
              u
              *
            
          
        
        
          
            
              λ
              *
            
          
        
      
      ]
    
  
  =
  
    [
    
      
        
          
            -
            c
          
        
      
      
        
          r
        
      
    
    ]
  



e)
e) generating commands based upon the solutions to the quadratic programming problem from said step d) and sending the commands to actuators to generate a physical input to the system.

19. The method of claim 18 further including the steps of:
Determining a largest αε[0,1] such that uk−1+αs is feasible, where s=u*−uk−1, u−1 denotes the prior iterate and u0=uf.

20. The method of claim 19 where α is given by




  α
  =
  
    
      min
      
        i
        ∉
        W
      
    
    ⁢
    
      
        
          
            b
            i
          
          -
          
            
              (
              
                Au
                
                  k
                  -
                  1
                
              
              )
            
            ⁢
            i
          
        
        
          As
          i
        
      
      .

21. The method of claim 20 further including the step of updating the guess for the active set in the next iterate.

22. The method of claim 21 further including the step of adding to W the first index iB corresponding to which α achieves its minimum value in the ratio test, the guess for the active indices in the next iterate.

23. The method of claim 22 further including the step of dropping the constraint with the most negative multiplier from the active set.

24. The method of claim 23 further including the step of: in the first iteration, dropping all constraints that are not consistent if α<1.

25. A model predictive control system comprising:
a plurality of sensor sampling current conditions of the system including a system output in a plurality of sample periods;
a desired trajectory generator for creating a desired trajectory based upon the current conditions of the system as sampled by the plurality of the sensors;
a linearization module deriving a linearized model about the desired trajectory;
a quadratic programming module in each of a plurality of time steps formulating a problem of achieving the desired trajectory for a multiple time window as a solution to a quadratic programming problem;
a quadratic programming solver for solving an optimization problem established by the quadratic programming module to generate a profile of optimal controls, the quadratic programming solver solving the quadratic programming problem in each time step using an iterative algorithm which searches for an optimal active set and in each subsequent time step of the plurality of time steps, initializing a search for the best active set based on a final active set of a prior time step of the plurality of time steps; and
an actuation system effecting the desired trajectory based upon a solution from the quadratic programming solver.

26. The system of claim 25 wherein the active set comprises a set of constraints that are binding at an optimized solution.

27. The system of claim 26 wherein the quadratic programming solver drops only one constraint with a negative Lagrange multipliers.

28. The system of claim 26 wherein the quadratic programming solver drops all constraints that are not binding irrespective of signs of Lagrange multipliers of the constraints.

29. The system of claim 25 the quadratic programming solver performs a QR factorization in each iterate of the iterative algorithm of said step e).

30. The system of claim 29 wherein a first iterate of the iterative algorithm includes performing a full QR factorization and subsequent iterates include performing a QR factorization update.

31. The system of claim 29 wherein the quadratic programming solver detects degeneracy during each QR factorization.

32. The system of claim 31 wherein the quadratic programming solver drops a degenerate constraint.

33. The system of claim 32 wherein the quadratic programming solver detects linear dependency in the active constraints in each of the iterations without pre-processing.

34. A method for controlling a multivariable system including the steps of:
a) sensing current conditions of the system and generating a plurality of sensor signals indicating the current conditions of the system;
b) receiving a plurality of commands;
c) in each of a plurality of time steps, formulating a problem of achieving a desired dynamic response for a window spanning multiple time steps as a solution to a quadratic programming problem based upon the commands and the sensor signals;
d) optimizing the quadratic programming problem of achieving a desired which searches for an optimal active set to solve the quadratic programming problem,
wherein said step d) include the steps of determining that an initial starting point set is not feasible and reformulating the problem to obtain a feasible iterate; and
e) controlling actuators to effect the desired dynamic response of the system based upon a solution to the quadratic programming problem from said step d).

35. The method of claim 34 wherein said step d) further includes the step of minimizing a weighted sum of the original QP problem and a measure of infeasibility.