CGI_ALLOC - Control allocation based on cascading generalized inverses. [u,iter] = cgi_alloc(B,v,umin,umax,[Wv,Wu,ud,imax]) Approximately 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 the method of cascading generalized inverses. 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] imax max no. of iterations [100] Outputs: -------- u (approximately) optimal solution iter no. of iterations (= no. of pseudoinverse solutions computed) See also: SLS_ALLOC, MLS_ALLOC, QP_SIM.
0001 function [u,iter] = cgi_alloc(B,v,umin,umax,Wv,Wu,ud,imax) 0002 0003 % CGI_ALLOC - Control allocation based on cascading generalized inverses. 0004 % 0005 % [u,iter] = cgi_alloc(B,v,umin,umax,[Wv,Wu,ud,imax]) 0006 % 0007 % Approximately 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 the method of cascading generalized inverses. 0016 % 0017 % Inputs: 0018 % ------- 0019 % B control effectiveness matrix (k x m) 0020 % v commanded virtual control (k x 1) 0021 % umin lower position limits (m x 1) 0022 % umax upper position limits (m x 1) 0023 % Wv virtual control weighting matrix (k x k) [I] 0024 % Wu control weighting matrix (m x m) [I] 0025 % ud desired control (m x 1) [0] 0026 % imax max no. of iterations [100] 0027 % 0028 % Outputs: 0029 % -------- 0030 % u (approximately) optimal solution 0031 % iter no. of iterations (= no. of pseudoinverse solutions computed) 0032 % 0033 % See also: SLS_ALLOC, MLS_ALLOC, QP_SIM. 0034 0035 % Set default values of optional arguments 0036 if nargin < 8 0037 imax = 100; % Heuristic value 0038 [k,m] = size(B); 0039 if nargin < 7, ud = zeros(m,1); end 0040 if nargin < 6, Wu = eye(m); end 0041 if nargin < 5, Wv = eye(k); end 0042 end 0043 0044 iter = 1; 0045 % Compute the optimal solution disregarding the inequality 0046 % constraints. 0047 A = Wv*B/Wu; 0048 b = Wv*(v-B*ud); 0049 u = ud + Wu\pinv_sol(A,b); 0050 0051 % Find indeces of infeasible variables. 0052 i_min = u < umin; 0053 i_max = u > umax; 0054 % Set these variables to their limits. 0055 u(i_min) = umin(i_min); 0056 u(i_max) = umax(i_max); 0057 % Let the remaining variables be free. 0058 i_free = ~(i_min | i_max); 0059 0060 % If the preceeding pseudoinverse solution yielded some variables 0061 % infeasible, redistribute the control effect to the remaining free 0062 % variables, if there are any. 0063 while any([i_min ; i_max]) & any(i_free) & (iter<imax); 0064 iter = iter + 1; 0065 % Now solve for optimal values of the remaining free variables. 0066 % See 2002-02-07. 0067 Wu1 = Wu(i_free,:); 0068 Wu11 = Wu1(:,i_free); 0069 B1 = B(:,i_free); 0070 0071 A = Wv*B1/Wu11; 0072 b = Wv*(v-B*u-B1*(Wu11\(Wu1*(ud-u)))); 0073 % Solve for optimal perturbation. 0074 p1 = Wu11\(pinv_sol(A,b)+Wu1*(ud-u)); 0075 % Update solution 0076 u(i_free) = u(i_free) + p1; 0077 0078 % Find indeces of _new_ infeasible components. 0079 i_min = u < umin; 0080 i_max = u > umax; 0081 % Set these variables to their limits. 0082 u(i_min) = umin(i_min); 0083 u(i_max) = umax(i_max); 0084 % Remove these from the free variables. 0085 i_free = i_free & ~(i_min | i_max); 0086 end