Description of sls_alloc

sls_alloc

PURPOSE

SLS_ALLOC - Control allocation using sequential least squares.

SYNOPSIS

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

DESCRIPTION

 SLS_ALLOC - Control allocation using sequential least squares.

  [u,W,iter] = sls_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. To handle the case of coplanar
 controls, pass B+i instead of just B.

  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) [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: MLS_ALLOC, CGI_ALLOC, QP_SIM, ISCOPLANAR.

CROSS-REFERENCE INFORMATION

This function calls:

• pinv_sol PINV_SOL - Compute pseudoinverse solution.

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] = sls_alloc(B,v,umin,umax,Wv,Wu,ud,u,W,imax)
0002   
0003 % SLS_ALLOC - Control allocation using sequential least squares.
0004 %
0005 %  [u,W,iter] = sls_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. To handle the case of coplanar
0016 % controls, pass B+i instead of just B.
0017 %
0018 %  Inputs:
0019 %  -------
0020 % B     control effectiveness matrix (k x m)
0021 % v     commanded virtual control (k x 1)
0022 % umin  lower position limits (m x 1)
0023 % umax  upper position limits (m x 1)
0024 % Wv    virtual control weighting matrix (k x k) [I]
0025 % Wu    control weighting matrix (m x m) [I]
0026 % ud    desired control (m x 1) [0]
0027 % u0    initial point (m x 1)
0028 % W0    initial working set (m x 1) [empty]
0029 % imax  max no. of iterations [100]
0030 %
0031 %  Outputs:
0032 %  -------
0033 % u     optimal control
0034 % W     optimal active set
0035 % iter  no. of iterations (= no. of changes in the working set + 1)
0036 %
0037 %                           0 if u_i not saturated
0038 % Active set syntax: W_i = -1 if u_i = umin_i
0039 %                          +1 if u_i = umax_i
0040 %
0041 % See also: MLS_ALLOC, CGI_ALLOC, QP_SIM, ISCOPLANAR.
0042   
0043 % Numerical tolerance
0044   tol = 1e-10;
0045   
0046   % Check if the coplanar safe version of the algorithm should be
0047   % used.
0048   coplanar = ~isreal(B);
0049   B = real(B);
0050   
0051   % k = number of virtual controls
0052   % m = number of variables (actuators)
0053   [k,m] = size(B);
0054 
0055   % Set default values of optional arguments
0056   if nargin < 10
0057     imax = 100; % Heuristic value
0058     if nargin < 9, u = (umin+umax)/2; W = zeros(m,1); end
0059     if nargin < 7,  ud = zeros(m,1); end
0060     if nargin < 6,  Wu = eye(m);     end
0061     if nargin < 5,  Wv = eye(k);     end
0062   end
0063 
0064   % Start with phase one.
0065   phase = 1;
0066   % Compute "A"-matrix and initial residual:
0067   % ||Wv(B(u0+p)-v)|| = ||Ap-d||
0068   A = Wv*B;
0069   d = Wv*(v-B*u);
0070   
0071   % Iterate until optimum is found or maximum number of iterations
0072   % is reached.
0073   for iter = 1:imax
0074     % ----------------------------------------
0075     %  Compute optimal perturbation vector p.
0076     % ----------------------------------------
0077     
0078     % Determine indeces of free variables
0079     i_free = W==0;
0080     % and number of free variables.
0081     m_free = sum(i_free);
0082     
0083     if phase == 1
0084       % ---------
0085       %  Phase 1
0086       % ---------
0087       
0088       % Eliminate saturated variables.
0089       A_free = A(:,i_free);
0090       
0091       % Solve the reduced optimization problem for free variables. If
0092       % A_free p_free = d does not have a unique least-squares
0093       % solution, pick the minimum length solution.
0094       if coplanar
0095     % Actuators are allowed to produce coplanar controls. This means
0096     % that A_free may not be of full rank even if m_free>k.
0097     p_free = pinv_sol(A_free,d);
0098       else
0099     % A_free will allways be of full (row or column) rank. This
0100     % leads to two different cases:
0101     if m_free <= k
0102       % A_free p_free = d has a unique least-squares solution.
0103       p_free = A_free\d;
0104     else
0105       % Pick minimum norm solution to A_free p_free = d.
0106       [Q1,R1] = qr(A_free',0);
0107       p_free = Q1*(R1'\d);
0108     end
0109       end 
0110       
0111       % Insert perturbations from p_free into free the variables.
0112       p = zeros(m,1);
0113       p(i_free) = p_free;
0114       
0115     else
0116       % ---------
0117       %  Phase 2
0118       % ---------
0119 
0120       % Determine indeces of fixed variables,
0121       i_fixed = find(W);
0122       % and number of fixed variables.
0123       m_fixed = length(i_fixed);
0124       % Construct C0 containing the active inequalities.
0125       C0 = zeros(m_fixed,m);
0126       for i = 1:m_fixed
0127     C0(i,i_fixed(i)) = -W(i_fixed(i));
0128       end
0129       % Construct E containing all equality constraints. By
0130       % construction, the rows of E (= the equality constraints) are
0131       % linearly independent.
0132       E = [B ; C0];
0133 
0134       % Number of  constraints.
0135       k_c = k + m_fixed;
0136       
0137       % Compute its QR decomposition E' = QR =(Q1 Q2)(R1)
0138       %                                              ( 0)
0139       % Note that the computation of lambda also uses this
0140       % decomposition.
0141       [Q,R] = qr(E');
0142       Q1 = Q(:,1:k_c);
0143       Q2 = Q(:,k_c+1:end);
0144       R1 = R(1:k_c,:);
0145       % Optimal solution:
0146       q2 = (A*Q2)\d;
0147       p = Q2*q2;
0148       
0149     end 
0150     % Optimal perturbation p computed.
0151     
0152     % --------------------------------
0153     %  Is the optimal point feasible?
0154     % --------------------------------
0155     
0156     u_opt = u + p;
0157     if ~any(u_opt(i_free) < umin(i_free) | u_opt(i_free) > umax(i_free))
0158       
0159       % ------
0160       %  Yes.
0161       % ------
0162       
0163       % Update point.
0164       u = u_opt;
0165       % Update residual.
0166       d = d - A*p;
0167       
0168       % --------------------------------------------------------
0169       %  Is the point the optimal solution to the full problem?
0170       % --------------------------------------------------------
0171     
0172       if (~coplanar) & (phase == 1) & (m_free >= k)
0173     % No planar controls. If u is feasible, then Bu=v must hold,
0174         % by construction, i.e., one phase 1 optimum found.
0175     % Move to phase 2.
0176     phase = 2;
0177     % Update "A"-matrix and residual.
0178     A = Wu;
0179     d = A*(ud-u);
0180       else
0181     % Check for optimality by computing the Lagrange multipliers.
0182     
0183     % Compute gradient of optimization criterion.
0184     g = -A'*d; % d = b-Au --> gradient g = A'(Au-b)
0185     
0186     % Compute lambda for all bounds (including inactive ones).
0187     if phase == 1
0188       % Only box constraints.
0189       lambda = -W.*g;
0190     else
0191       % Box constraints and equality constraints. Use QR decomp.
0192       Lambda = R1\(Q1'*g);
0193       % Extract multipliers related to bounds.
0194       lambda = zeros(m,1);
0195       lambda(i_fixed) = Lambda(k+1:end);
0196     end
0197     
0198     if lambda >= -tol
0199       % All relevant Lagrange multipliers are positive.
0200       if coplanar & (phase == 1)
0201         % Although ||Wv(Bu-v)|| is minimal, ||Wu(u-ud)|| may be
0202             % reduced further in phase 2.
0203         phase = 2;
0204         % Update "A"-matrix and residual.
0205         % ||Wu(u+p-ud)|| = ||Ap-d||
0206         A = Wu;
0207         d = Wu*(ud-u);
0208         % Caveat: Adding the constraint Bp = 0 in phase 2 may
0209             % cause linear dependence among the active constraints.
0210         % Therefore remove the active box constraints causing
0211             % such linear dependence.
0212         %
0213         % Not found a way to do this with quickly with
0214             % precision --> use "brute force" and empty the working
0215             % set.
0216         W = zeros(m,1);
0217       else
0218         % / ------------------------ \
0219         % | Optimum found, bail out. |
0220         % \ ------------------------ /
0221         return;
0222       end
0223     else
0224       
0225       % --------------------------------------------------
0226       %  Optimum not found, remove one active constraint.
0227       % --------------------------------------------------
0228       
0229       % Remove constraint with most negative lambda from the
0230       % working set.
0231       [lambda_neg,i_neg] = min(lambda);
0232       W(i_neg) = 0;
0233     end % lambda >= 0
0234       end    
0235     else % feasible?
0236       
0237       % ------------------------------------------------------------
0238       %  No (point not feasible), find primary bounding constraint.
0239       % ------------------------------------------------------------
0240       
0241       % Compute distances to the different boundaries. Since alpha < 1
0242       % is the maximum step length, initiate with ones.
0243       dist = ones(m,1);
0244       i_min = i_free & p<-tol; % rather than <0
0245       i_max = i_free & p>tol;
0246       
0247       dist(i_min) = (umin(i_min) - u(i_min)) ./ p(i_min);
0248       dist(i_max) = (umax(i_max) - u(i_max)) ./ p(i_max);
0249       
0250       % Proportion of p to travel.
0251       [alpha,i_alpha] = min(dist);
0252       % Update point and residual.
0253       u = u + alpha*p;
0254       d = d - A*alpha*p;
0255       
0256       % Add corresponding constraint to working set.
0257       W(i_alpha) = sign(p(i_alpha));
0258       
0259     end
0260     
0261   end

---

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