FXP_ALLOC - Control allocation using a fixed-point iterations. [u,iter] = fxp_alloc(B,v,umin,umax,[Wv,Wu,ud,gamma,u0,imax]) Solves the weighted, bounded least-squares problem min ||Wu(u-ud)||^2 + gamma ||Wv(Bu-v)||^2 subj. to umin <= u <= umax using a fixed-point iteration algorithm. 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) [1e3] u0 initial point (m x 1) imax no. of iterations [100] Outputs: ------- u (approximately) optimal solution iter no. of iterations See also: WLS_ALLOC, WLSC_ALLOC, IP_ALLOC, QP_SIM.
0001 function [u,iter] = fxp_alloc(B,v,umin,umax,Wv,Wu,ud,gam,u,imax) 0002 0003 % FXP_ALLOC - Control allocation using a fixed-point iterations. 0004 % 0005 % [u,iter] = fxp_alloc(B,v,umin,umax,[Wv,Wu,ud,gamma,u0,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 a fixed-point iteration algorithm. 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) [1e3] 0025 % u0 initial point (m x 1) 0026 % imax no. of iterations [100] 0027 % 0028 % Outputs: 0029 % ------- 0030 % u (approximately) optimal solution 0031 % iter no. of iterations 0032 % 0033 % See also: WLS_ALLOC, WLSC_ALLOC, IP_ALLOC, QP_SIM. 0034 0035 % Number of variables 0036 m = length(umin); 0037 0038 % Set default values of optional arguments 0039 if nargin < 10 0040 imax = 100; % Heuristic value 0041 [k,m] = size(B); 0042 if nargin < 9, u = (umin+umax)/2; end 0043 if nargin < 8, gam = 1e3; end 0044 if nargin < 7, ud = zeros(m,1); end 0045 if nargin < 6, Wu = eye(m); end 0046 if nargin < 5, Wv = eye(k); end 0047 end 0048 0049 % Change of variables. 0050 x = u - ud; 0051 xmin = umin - ud; 0052 xmax = umax - ud; 0053 0054 % Compute the weight epsilon used by Burken et al. 0055 e = 1/(gam+1); 0056 0057 % m = number of variables (actuators) 0058 m = size(B,2); 0059 0060 % Notational differences: Q1 = Wu'Wu 0061 % Q2 = Wv'Wv 0062 % CzBu*-dz = v 0063 0064 % Common factor. 0065 BzT_Q1 = B'*Wv'*Wv; 0066 % Just above eq. (25). 0067 H = (1-e) * (BzT_Q1 * B) + e*Wu'*Wu; 0068 % Eq. (25). 0069 w = 1/norm(H,'fro'); 0070 % Matrices used for updating solution: 0071 % u[k] = sat(Fv-Gu[k-1]) 0072 Fv = (1-e) * w * BzT_Q1 * v; 0073 G = w*H - eye(m); 0074 0075 for iter = 1:imax 0076 % Compute next point without considering the constraints. 0077 x = Fv - G*x; 0078 % Saturate to box constraints. 0079 x = max(xmin,min(xmax,x)); 0080 end 0081 0082 % Back to original variables. 0083 u = x + ud;