Description of alloc

0001 function [u,A,time,iter] = alloc_sim(method,B,v,plim,rlim,T,varargin)
0002 
0003 % ALLOC_SIM - Control allocation simulation.
0004 %
0005 %  [u,A,iter,time] = alloc_sim(method,B,v,plim,rlim,T,varargin)
0006 %
0007 % Subroutine not intended to be called directly by user.
0008   
0009 % Performs control allocation for a sequence of virtual control
0010 % commands stored in v.
0011 %
0012 %  Inputs:
0013 %  -------
0014 % method   control allocation method: 'qp'   l2-optimal allocation
0015 %                      'dyn'  dynamic allocation
0016 %                      'dir'  direct allocation
0017 % B       control effectiveness matrix (k x m)
0018 % v        commanded virtual control trajectory (k x N)
0019 % plim     position limits [min max] (m x 2)
0020 % rlim       rate limit = [min max] (m x 2)
0021 % T       sampling time
0022 % options  options, see code
0023 %
0024 %  Outputs:
0025 %  -------
0026 % u     optimal control
0027 % A     active constraints in u
0028 % iter  no. of iterations
0029 % time  average computation time per sample
0030   
0031 % Number of variables
0032   [k,m] = size(B);
0033   % Coplanar controls? Only in SLS_ALLOC
0034   copl = iscoplanar(B);
0035   % Number of samples
0036   N = size(v,2);
0037   % Allocate output variables
0038   u = zeros(m,N);
0039   A = zeros(m,N);
0040   iter = zeros(1,N);
0041   time = zeros(1,N);
0042   
0043   % Set default values of optional arguments
0044   switch method
0045    case {'qp','dyn'}
0046     alg  = 'wls';
0047    case 'dir'
0048     alg = 'dir';
0049   end
0050   M    = 1;         % no of repetitions
0051   imax = 100;         % no of iterations
0052   gam  = 1e6;         % weight
0053   tol  = 1e-6;       % tolerance in IP solver
0054   hs   = 1;         % hotstart
0055   ui   = [];         % initial control
0056   Wi   = zeros(m,1); % initial working set
0057   Wv   = eye(k);     % QP allocation
0058   Wu   = eye(m);
0059   ud   = zeros(m);
0060   W1   = eye(m);     % Dynamic allocation
0061   W2   = zeros(m);
0062   S    = pinv(B);
0063   
0064   for i=1:2:length(varargin)
0065     switch(varargin{i})
0066       % --- General options ---
0067      case 'alg'  % solver, algorithm
0068       alg = varargin{i+1};
0069      case 'rep'  % no of repetitions
0070       M = varargin{i+1};
0071 
0072       % --- Method specific options ---
0073      case 'imax' % number of iterations
0074       imax = varargin{i+1};
0075      case 'gam'  % LS weight
0076       gam = varargin{i+1};
0077      case 'tol'  % IP tolerance
0078       tol = varargin{i+1};
0079      case 'hot'  % hotstart
0080       hs = varargin{i+1};
0081      case 'ui'   % active set (also fix-point)
0082       ui = varargin{i+1};
0083      case 'Wi'
0084       Wi = varargin{i+1};
0085      case 'Wv'   % QP allocation
0086       Wv = varargin{i+1};
0087      case 'Wu'
0088       Wu = varargin{i+1};
0089      case 'ud'
0090       ud = varargin{i+1};
0091      case 'W1'   % dynamic allocation
0092       W1 = varargin{i+1};
0093      case 'W2'
0094       W2 = varargin{i+1};
0095      case 'S'
0096       S = varargin{i+1};
0097      
0098      otherwise
0099       error(sprintf('Unknown option: %s',varargin{i}));
0100     end
0101   end
0102 
0103   if strcmp(alg,'ip')
0104     % Weighting matrices must be unit matrices.
0105     if any(any(Wv ~= eye(k))) | any(any(Wu ~= eye(m)))
0106       disp(' ')
0107       disp(['** Warning: Non-unit matrices Wv and Wu not handled by' ...
0108         ' IP_ALLOC **']) 
0109       disp(' ')
0110     end
0111     % Dynamic allocation almost always requires non-unit matrix Wu.
0112     if strcmp(method,'dyn')
0113       disp(' ')
0114       disp(['** Warning: Dynamic allocation not handled by IP_ALLOC,' ...
0115         ' resorting to WLS_ALLOC **'])
0116       disp(' ');
0117       alg = 'wls';
0118     end
0119   end
0120   
0121   if isempty(ui)
0122     % Set default initial conditions
0123     switch method
0124      case 'qp'
0125       [ui,Wi] = wls_alloc(B,v(:,1),plim(:,1),plim(:,2),Wv,Wu,ud);
0126      case 'dyn'
0127       [ui,Wi] = wls_alloc(B,v(:,1),plim(:,1),plim(:,2),Wv,W1,S*v(:,1));
0128      case 'dir'
0129       ui = dir_alloc(B,v(:,1),plim(:,1),plim(:,2));
0130     end
0131   end
0132   
0133   % Hotstart?
0134   if hs
0135     u0 = ui;
0136     W0 = Wi;
0137   end
0138   
0139   % Precompute matrices used in dynamic allocation
0140   if strcmp(method,'dyn')
0141     W1sq = W1^2;
0142     W2sq = W2^2;
0143     Wu = sqrtm(W1sq+W2sq);
0144     invWusq = inv(W1sq+W2sq);
0145   end
0146   
0147   % Allocate control signals to produce the demanded virtual
0148   % control trajectory.
0149   uprev = ui;
0150   for i=1:N
0151     % Compute feasible upper and lower bounds.
0152     if isempty(rlim)
0153       umin = plim(:,1);
0154       umax = plim(:,2);
0155     else
0156       umin = max(plim(:,1),uprev+rlim(:,1)*T);
0157       umax = min(plim(:,2),uprev+rlim(:,2)*T);
0158     end
0159     if ~hs
0160       u0 = (umin+umax)/2;
0161       W0 = zeros(m,1);
0162     end
0163     % Update u0 to reflect working set. Crucial when rate limits
0164     % are included and hotstart is used.
0165     i_min = W0 == -1;
0166     i_max = W0 == +1;
0167     u0(i_min) = umin(i_min);
0168     u0(i_max) = umax(i_max);
0169     % For dynamic allocation, merge the position and rate terms
0170     % into one term ||Wu(u-ud)||.
0171     if strcmp(method,'dyn')
0172       us = S*v(:,i);
0173       ud = invWusq*(W1sq*us+W2sq*uprev);
0174     end
0175     % Solve control allocation problem M times
0176     tic;
0177     for l = 1:M
0178       switch alg
0179        case 'sls'
0180     [u(:,i),W,iter(i)] = ...
0181         sls_alloc(B+copl*j,v(:,i),umin,umax,Wv,Wu,ud,u0,W0,imax);
0182        case 'mls'
0183     [u(:,i),W,iter(i)] = ...
0184         mls_alloc(B,v(:,i),umin,umax,Wv,Wu,ud,u0,W0,imax);
0185        case 'wls'
0186     [u(:,i),W,iter(i)] = ...
0187         wls_alloc(B,v(:,i),umin,umax,Wv,Wu,ud,gam,u0,W0,imax);
0188     % --- FoT25 allocation routine, not in standard QCAT ---
0189        case 'wls_c'
0190     [u(:,i),W,iter(i)] = ...
0191         wls_alloc_c(B,v(:,i),umin,umax,Wv,Wu,ud,gam,u0,W0,imax);
0192     % ------------------------------------------------------
0193        case 'wlsc'
0194     [u(:,i),W,iter(i)] = ...
0195         wlsc_alloc(B,v(:,i),umin,umax,Wv,Wu,ud,gam,u0,W0,imax);
0196        case 'ip'
0197     [u(:,i),iter(i)] = ...
0198         ip_alloc(B,v(:,i),umin,umax,ud,gam,tol,imax);
0199        case 'fxp'
0200     u(:,i) = fxp_alloc(B,v(:,i),umin,umax,Wv,Wu,ud,gam,u0,imax);
0201        case 'cgi'
0202     u(:,i) = cgi_alloc(B,v(:,i),umin,umax,Wv,Wu,ud,imax);
0203        case 'dir'
0204     u(:,i) = dir_alloc(B,v(:,i),plim(:,1),plim(:,2));
0205        otherwise
0206     error(sprintf('Unknown allocation algorithm: %s',alg));
0207       end
0208     end
0209     % Register elapsed time
0210     time(i) = toc/M;
0211     % Limit control (only necessary with rate limits and direct alloc)
0212     u(:,i) = max(umin,min(umax,u(:,i)));
0213     % Determine active constraints in the final point.
0214     % +/- : max or min
0215     % 1/2 : position or rate limit
0216     A(:,i) = - 2*(u(:,i)==umin) + (u(:,i)==plim(:,1)) ...
0217          + 2*(u(:,i)==umax) - (u(:,i)==plim(:,2));
0218     % Update uprev
0219     uprev = u(:,i);
0220     % Hotstart?
0221     if hs
0222       u0 = u(:,i);
0223       if any(strcmp(alg,{'sls','wls','wls_c','wlsc','mls'}))
0224     W0 = W;
0225       end
0226     end
0227   end
alloc_sim

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
