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