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.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;