WLS_ALLOC - Control allocation using weighted least squares.
[u,W,iter] = wls_alloc(B,v,umin,umax,[Wv,Wu,ud,gamma,u0,W0,imax])
Solves the weighted, bounded least-squares problem
min ||Wu(u-ud)||^2 + gamma ||Wv(Bu-v)||^2
subj. to umin <= u <= umax
using an active set method.
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]
gamma weight (scalar) [1e6]
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
Working set syntax: W_i = -1 if u_i = umin_i
+1 if u_i = umax_i
See also: WLSC_ALLOC, IP_ALLOC, FXP_ALLOC, QP_SIM.0001 function [u,W,iter] = wls_alloc(B,v,umin,umax,Wv,Wu,ud,gam,u,W,imax) 0002 0003 % WLS_ALLOC - Control allocation using weighted least squares. 0004 % 0005 % [u,W,iter] = wls_alloc(B,v,umin,umax,[Wv,Wu,ud,gamma,u0,W0,imax]) 0006 % 0007 % Solves the weighted, bounded least-squares problem 0008 % 0009 % min ||Wu(u-ud)||^2 + gamma ||Wv(Bu-v)||^2 0010 % 0011 % subj. to umin <= u <= umax 0012 % 0013 % using an active set method. 0014 % 0015 % Inputs: 0016 % ------- 0017 % B control effectiveness matrix (k x m) 0018 % v commanded virtual control (k x 1) 0019 % umin lower position limits (m x 1) 0020 % umax upper position limits (m x 1) 0021 % Wv virtual control weighting matrix (k x k) [I] 0022 % Wu control weighting matrix (m x m) [I] 0023 % ud desired control (m x 1) [0] 0024 % gamma weight (scalar) [1e6] 0025 % u0 initial point (m x 1) 0026 % W0 initial working set (m x 1) [empty] 0027 % imax max no. of iterations [100] 0028 % 0029 % Outputs: 0030 % ------- 0031 % u optimal control 0032 % W optimal active set 0033 % iter no. of iterations (= no. of changes in the working set + 1) 0034 % 0035 % 0 if u_i not saturated 0036 % Working set syntax: W_i = -1 if u_i = umin_i 0037 % +1 if u_i = umax_i 0038 % 0039 % See also: WLSC_ALLOC, IP_ALLOC, FXP_ALLOC, QP_SIM. 0040 0041 % Number of variables 0042 m = length(umin); 0043 0044 % Set default values of optional arguments 0045 if nargin < 11 0046 imax = 100; % Heuristic value 0047 [k,m] = size(B); 0048 if nargin < 10, u = (umin+umax)/2; W = zeros(m,1); end 0049 if nargin < 8, gam = 1e6; end 0050 if nargin < 7, ud = zeros(m,1); end 0051 if nargin < 6, Wu = eye(m); end 0052 if nargin < 5, Wv = eye(k); end 0053 end 0054 0055 gam_sq = sqrt(gam); 0056 A = [gam_sq*Wv*B ; Wu]; 0057 b = [gam_sq*Wv*v ; Wu*ud]; 0058 0059 % Initial residual. 0060 d = b - A*u; 0061 % Determine indeces of free variables. 0062 i_free = W==0; 0063 0064 % Iterate until optimum is found or maximum number of iterations 0065 % is reached. 0066 for iter = 1:imax 0067 % ---------------------------------------- 0068 % Compute optimal perturbation vector p. 0069 % ---------------------------------------- 0070 0071 % Eliminate saturated variables. 0072 A_free = A(:,i_free); 0073 % Solve the reduced optimization problem for free variables. 0074 p_free = A_free\d; 0075 % Zero all perturbations corresponding to active constraints. 0076 p = zeros(m,1); 0077 % Insert perturbations from p_free into free the variables. 0078 p(i_free) = p_free; 0079 0080 % ---------------------------- 0081 % Is the new point feasible? 0082 % ---------------------------- 0083 0084 u_opt = u + p; 0085 infeasible = (u_opt < umin) | (u_opt > umax); 0086 0087 if ~any(infeasible(i_free)) 0088 0089 % ---------------------------- 0090 % Yes, check for optimality. 0091 % ---------------------------- 0092 0093 % Update point and residual. 0094 u = u_opt; 0095 d = d - A_free*p_free; 0096 % Compute Lagrangian multipliers. 0097 lambda = W.*(A'*d); 0098 % Are all lambda non-negative? 0099 if lambda >= -eps 0100 % / ------------------------ \ 0101 % | Optimum found, bail out. | 0102 % \ ------------------------ / 0103 return; 0104 end 0105 0106 % -------------------------------------------------- 0107 % Optimum not found, remove one active constraint. 0108 % -------------------------------------------------- 0109 0110 % Remove constraint with most negative lambda from the 0111 % working set. 0112 [lambda_neg,i_neg] = min(lambda); 0113 W(i_neg) = 0; 0114 i_free(i_neg) = 1; 0115 0116 else 0117 0118 % --------------------------------------- 0119 % No, find primary bounding constraint. 0120 % --------------------------------------- 0121 0122 % Compute distances to the different boundaries. Since alpha < 1 0123 % is the maximum step length, initiate with ones. 0124 dist = ones(m,1); 0125 i_min = i_free & p<0; 0126 i_max = i_free & p>0; 0127 0128 dist(i_min) = (umin(i_min) - u(i_min)) ./ p(i_min); 0129 dist(i_max) = (umax(i_max) - u(i_max)) ./ p(i_max); 0130 0131 % Proportion of p to travel 0132 [alpha,i_alpha] = min(dist); 0133 % Update point and residual. 0134 u = u + alpha*p; 0135 d = d - A_free*alpha*p_free; 0136 0137 % Add corresponding constraint to working set. 0138 W(i_alpha) = sign(p(i_alpha)); 0139 i_free(i_alpha) = 0; 0140 0141 end 0142 0143 end