Description of mls_alloc

mls_alloc

PURPOSE

MLS_ALLOC - Control allocation using minimal least squares.

SYNOPSIS

function [u,W,iter] = mls_alloc(B,v,umin,umax,Wv,Wu,ud,u,W,imax)

DESCRIPTION

 MLS_ALLOC - Control allocation using minimal least squares.

  [u,W,iter] = mls_alloc(B,v,umin,umax,[Wv,Wu,ud,u0,W0,imax])

 Solves the bounded sequential least-squares problem

   min ||Wu(u-ud)||   subj. to   u in M

 where M is the set of control signals solving

   min ||Wv(Bu-v)||   subj. to   umin <= u <= umax

 using a two stage active set method. Wu must be diagonal since the
 problem is reformulated as a minimal least squares problem. The
 implementation does not handle the case of coplanar controls.

  Inputs:
  -------
 B     control effectiveness matrix (k x m)
 v     commanded virtual control (k x 1)
 umin  lower position limits (m x 1)
 umax  upper position limits (m x 1)
 Wv    virtual control weighting matrix (k x k) [I]
 Wu    control weighting matrix (m x m), diagonal [I]
 ud    desired control (m x 1) [0]
 u0    initial point (m x 1)
 W0    initial working set (m x 1) [empty]
 imax  max no. of iterations [100]
 
  Outputs:
  -------
 u     optimal control
 W     optimal active set
 iter  no. of iterations (= no. of changes in the working set + 1)

                           0 if u_i not saturated
 Active set syntax: W_i = -1 if u_i = umin_i
                          +1 if u_i = umax_i

 See also: SLS_ALLOC, CGI_ALLOC, QP_SIM.

CROSS-REFERENCE INFORMATION

This function calls:

This function is called by:

• alloc_sim ALLOC_SIM - Control allocation simulation.
• qp_ca_sl Wrapper used in the QP control allocation Simulink block.

SOURCE CODE

0001 function [u,W,iter] = mls_alloc(B,v,umin,umax,Wv,Wu,ud,u,W,imax)
0002   
0003 % MLS_ALLOC - Control allocation using minimal least squares.
0004 %
0005 %  [u,W,iter] = mls_alloc(B,v,umin,umax,[Wv,Wu,ud,u0,W0,imax])
0006 %
0007 % Solves the bounded sequential least-squares problem
0008 %
0009 %   min ||Wu(u-ud)||   subj. to   u in M
0010 %
0011 % where M is the set of control signals solving
0012 %
0013 %   min ||Wv(Bu-v)||   subj. to   umin <= u <= umax
0014 %
0015 % using a two stage active set method. Wu must be diagonal since the
0016 % problem is reformulated as a minimal least squares problem. The
0017 % implementation does not handle the case of coplanar controls.
0018 %
0019 %  Inputs:
0020 %  -------
0021 % B     control effectiveness matrix (k x m)
0022 % v     commanded virtual control (k x 1)
0023 % umin  lower position limits (m x 1)
0024 % umax  upper position limits (m x 1)
0025 % Wv    virtual control weighting matrix (k x k) [I]
0026 % Wu    control weighting matrix (m x m), diagonal [I]
0027 % ud    desired control (m x 1) [0]
0028 % u0    initial point (m x 1)
0029 % W0    initial working set (m x 1) [empty]
0030 % imax  max no. of iterations [100]
0031 %
0032 %  Outputs:
0033 %  -------
0034 % u     optimal control
0035 % W     optimal active set
0036 % iter  no. of iterations (= no. of changes in the working set + 1)
0037 %
0038 %                           0 if u_i not saturated
0039 % Active set syntax: W_i = -1 if u_i = umin_i
0040 %                          +1 if u_i = umax_i
0041 %
0042 % See also: SLS_ALLOC, CGI_ALLOC, QP_SIM.
0043   
0044 % k = number of virtual controls
0045 % m = number of variables (actuators)
0046   [k,m] = size(B);
0047   
0048   % Set default values of optional arguments
0049   if nargin < 10
0050     imax = 100; % Heuristic value
0051     if nargin < 9, u = (umin+umax)/2; W = zeros(m,1); end
0052     if nargin < 7,  ud = zeros(m,1); end
0053     if nargin < 6,  Wu = eye(m);     end
0054     if nargin < 5,  Wv = eye(k);     end
0055   end
0056 
0057   % Start with phase one.
0058   phase = 1;
0059   
0060   % Reformulate as a minimal least squares problem. See 2002-03-08 (1).
0061   A = Wv*B/Wu;
0062   b = Wv*(v-B*ud); % Note sign convention: min ||Ax-b||
0063   xmin = Wu*(umin-ud);
0064   xmax = Wu*(umax-ud);
0065   % Compute initial point and residual.
0066   x = Wu*(u-ud);
0067   r = A*x-b;
0068   % Determine indeces of free variables
0069   i_free = W==0;
0070   % and number of free variables.
0071   m_free = sum(i_free);
0072   
0073   % Iterate until optimum is found or maximum number of iterations
0074   % is reached.
0075   for iter = 1:imax
0076     % ----------------------------------------
0077     %  Compute optimal perturbation vector p.
0078     % ----------------------------------------
0079     
0080     if phase == 1
0081       % ---------
0082       %  Phase 1
0083       % ---------
0084 
0085       % Eliminate saturated variables.
0086       A_free = A(:,i_free);
0087 
0088       % Solve the reduced optimization problem for free variables.
0089       % Under the assumption that no n actuators produce coplanar
0090       % control efforts, A_free will allways be of full rank. This
0091       % leads to two different cases:
0092       if m_free <= k
0093     % --------------------------------------------------
0094     %  min ||A_free p_free + r|| has a unique solution.
0095     % --------------------------------------------------
0096     p_free = -A_free\r;
0097       else
0098     % ---------------------------------------------------
0099     %  min ||A_free p_free + r|| has no unique solution.
0100     %
0101     %  Pick the minimal solution.
0102     % ---------------------------------------------------
0103 
0104     % QR decompose: A_free' = QR = (Q1 Q2)(R1)
0105     %                                     ( 0)
0106     [Q1,R1] = qr(A_free',0);
0107     p_free = -Q1*(R1'\r);
0108       end
0109       
0110       % Insert perturbations from p_free into free the variables.
0111       p = zeros(m,1);
0112       p(i_free) = p_free;
0113 
0114     else
0115       % ---------
0116       %  Phase 2
0117       % ---------
0118       
0119       % Determine indeces of fixed variables,
0120       i_fixed = ~i_free;
0121       % and number of fixed variables.
0122       m_fixed = m - m_free;
0123 
0124       % HT' = rows of U in working set.
0125       % HT = (m - n) x i_fixed
0126       HT = U(i_fixed,:)';
0127       % QR decompose: HT = VRtot = (V1 V2)(R)
0128       %                          (0)
0129       % Note that the computation of lambda also uses this
0130       % decomposition.
0131       [V,Rtot] = qr(HT);
0132       V1 = V(:,1:m_fixed);
0133       V2 = V(:,m_fixed+1:end);
0134       R = Rtot(1:m_fixed,:);
0135       % Optimal solution:
0136       s = -V2'*z;
0137       pz = V2*s;
0138       p = U*pz;
0139       
0140     end % Optimal perturbation p computed.
0141     
0142     % --------------------------------
0143     %  Is the optimal point feasible?
0144     % --------------------------------
0145     
0146     x_opt = x + p;
0147     infeasible = (x_opt < xmin) | (x_opt > xmax);
0148 
0149     if ~any(infeasible(i_free))
0150     
0151       % ------
0152       %  Yes.
0153       % ------
0154       
0155       % Update point and residual
0156       x = x_opt;
0157       if phase == 1
0158     r = r + A*p;
0159       else
0160     z = z + pz;
0161       end
0162       
0163       if (phase == 1) & (m_free >= k)
0164     % If u is feasible, then Bu=v must hold, by construction.
0165     % Move to phase 2.
0166     phase = 2;
0167 
0168     % QR decompose A'.
0169     [Utot,Stot] = qr(A');
0170     U = Utot(:,k+1:end);
0171     % Initial point.
0172     z = U'*x;
0173     
0174       else
0175     
0176     % Check for optimality by computing the Lagrange multipliers.
0177     % Compute lambda for all bounds (including inactive ones).
0178     lambda = zeros(m,1);
0179 
0180     if m_free < m
0181       if phase == 1
0182         % Compute gradient of optimization criterion.
0183         g = A'*r;
0184         % Compute Lagrange variables.
0185         lambda = -W.*g;
0186         
0187       else
0188         % Insert multipliers related to active bounds.
0189         lambda(i_fixed) = -W(i_fixed).*(R\(V1'*z));
0190       end
0191     end
0192       
0193     if lambda >= -eps
0194       % / ------------------------ \
0195       % | Optimum found, bail out. |
0196       % \ ------------------------ /
0197 
0198       % Compute solution in original coordinates.
0199       u = Wu\x + ud;
0200       return;
0201     end
0202     
0203     % --------------------------------------------------
0204     %  Optimum not found, remove one active constraint.
0205     % --------------------------------------------------
0206     
0207     % Remove constraint with most negative lambda from the
0208         % working set.
0209     [lambda_neg,i_neg] = min(lambda);
0210     W(i_neg) = 0;
0211     i_free(i_neg) = 1;
0212     m_free = m_free + 1;
0213       end
0214     else
0215       
0216       % ------------------------------------------------------------
0217       %  No (point not feasible), find primary bounding constraint.
0218       % ------------------------------------------------------------
0219       
0220       % Compute distances to the different boundaries. Since alpha < 1
0221       % is the maximum step length, initiate with ones.
0222       dist = ones(m,1);
0223       i_min = i_free & p<0;
0224       i_max = i_free & p>0;
0225       
0226       dist(i_min) = (xmin(i_min) - x(i_min)) ./ p(i_min);
0227       dist(i_max) = (xmax(i_max) - x(i_max)) ./ p(i_max);
0228       
0229       % Proportion of p to travel.
0230       [alpha,i_alpha] = min(dist);
0231 
0232       % Update point (and residual).
0233       x = x + alpha*p;
0234       if phase == 1
0235     r = r + A*alpha*p;
0236       else
0237     z = z + alpha*pz;
0238       end
0239     
0240       % Add corresponding constraint to working set.
0241       W(i_alpha) = sign(p(i_alpha));
0242       i_free(i_alpha) = 0;
0243       m_free = m_free - 1;
0244       
0245     end
0246     
0247   end
0248   
0249   % Compute solution in original coordinates.
0250   u = Wu\x + ud;

---

Generated on Wed 25-Aug-2004 14:38:35 by m2html © 2003