QP_SIM - QP control allocation simulation. [u,W,time,iter] = qp_sim(B,v,plim,[rlim,T,Wv,Wu,ud],options) Performs l2-optimal control allocation for a sequence of virtual control commands stored in v. For each value of v, the control signal u is determined by solving the quadratic program min ||Wu(u-ud)|| subj. to u in M where M is the set of control signals solving min ||Wv(Bu-v)|| subj. to u in U where U is the set of feasible control signals with respect to position limits and, optionally, rate limits. Inputs: ------- B control effectiveness matrix (k x m) v commanded virtual control trajectory (k x N) plim position limits [min max] (m x 2) rlim rate limits [min max] (m x 2) ([] --> no rate limits) T sampling time [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] Options: options = option_1,value_1,option_2,value_2,... -------- 'alg' numerical algorithm: 'sls' SLS_ALLOC 'wls' WLS_ALLOC (default) 'wlsc' WLSC_ALLOC 'mls' MLS_ALLOC 'ip' IP_ALLOC 'cgi' CGI_ALLOC 'fxp' FXP_ALLOC 'imax' max no. of iterations [100] 'gam' weight used in algorithms based on weighted LS [1e6] 'tol' tolerance used in IP_ALLOC stopping criterion [1e-6] 'hot' hotstart solver (not ip/cgi) with previous solution (0/[1]) 'ui' initial control signal 'Wi' initial active constraints 'rep' no. of repetitions [1] Outputs: ------- u optimal control W active constraints in u (+/- : max/min, 1/2 : position/rate) time average computation time per sample iter no. of iterations Flight example: load admiredata u=qp_sim(B,v,plim,rlim,T); figure(1),plot(t,u*180/pi),legend('can','elev (r)','elev (l)','rud') figure(2),plot(t,B*u,t,v,'k--'),legend('roll','pitch','yaw') See also: DIR_SIM, DYN_SIM, and the allocation algorithms above.
0001 function [u,W,time,iter] = qp_sim(B,v,plim,varargin) 0002 0003 % QP_SIM - QP control allocation simulation. 0004 % 0005 % [u,W,time,iter] = qp_sim(B,v,plim,[rlim,T,Wv,Wu,ud],options) 0006 % 0007 % Performs l2-optimal control allocation for a sequence of virtual 0008 % control commands stored in v. For each value of v, the control 0009 % signal u is determined by solving the quadratic program 0010 % 0011 % min ||Wu(u-ud)|| subj. to u in M 0012 % 0013 % where M is the set of control signals solving 0014 % 0015 % min ||Wv(Bu-v)|| subj. to u in U 0016 % 0017 % where U is the set of feasible control signals with respect to 0018 % position limits and, optionally, rate limits. 0019 % 0020 % Inputs: 0021 % ------- 0022 % B control effectiveness matrix (k x m) 0023 % v commanded virtual control trajectory (k x N) 0024 % plim position limits [min max] (m x 2) 0025 % rlim rate limits [min max] (m x 2) ([] --> no rate limits) 0026 % T sampling time [1] 0027 % Wv virtual control weighting matrix (k x k) [I] 0028 % Wu control weighting matrix (m x m) [I] 0029 % ud desired control (m x 1) [0] 0030 % 0031 % Options: options = option_1,value_1,option_2,value_2,... 0032 % -------- 0033 % 'alg' numerical algorithm: 'sls' SLS_ALLOC 0034 % 'wls' WLS_ALLOC (default) 0035 % 'wlsc' WLSC_ALLOC 0036 % 'mls' MLS_ALLOC 0037 % 'ip' IP_ALLOC 0038 % 'cgi' CGI_ALLOC 0039 % 'fxp' FXP_ALLOC 0040 % 'imax' max no. of iterations [100] 0041 % 'gam' weight used in algorithms based on weighted LS [1e6] 0042 % 'tol' tolerance used in IP_ALLOC stopping criterion [1e-6] 0043 % 'hot' hotstart solver (not ip/cgi) with previous solution (0/[1]) 0044 % 'ui' initial control signal 0045 % 'Wi' initial active constraints 0046 % 'rep' no. of repetitions [1] 0047 % 0048 % Outputs: 0049 % ------- 0050 % u optimal control 0051 % W active constraints in u (+/- : max/min, 1/2 : position/rate) 0052 % time average computation time per sample 0053 % iter no. of iterations 0054 % 0055 % Flight example: 0056 % load admiredata 0057 % u=qp_sim(B,v,plim,rlim,T); 0058 % figure(1),plot(t,u*180/pi),legend('can','elev (r)','elev (l)','rud') 0059 % figure(2),plot(t,B*u,t,v,'k--'),legend('roll','pitch','yaw') 0060 % 0061 % See also: DIR_SIM, DYN_SIM, and the allocation algorithms above. 0062 0063 % F18 example 0064 % load f18data 0065 % u=qp_sim(B,v,plim,rlim,T1); 0066 % plot(tn,u) 0067 % plot(tn,v,'k',tn,B*u) 0068 0069 % Number of variables 0070 [k,m] = size(B); 0071 0072 % Find no. of optional arguments (excluding options) 0073 iopt = length(varargin)+1; 0074 for i = 1:length(varargin) 0075 if ischar(varargin{i}) 0076 iopt = i; % index of first option string 0077 break; 0078 end 0079 end 0080 narg = iopt-1; 0081 0082 % Set default values of optional arguments 0083 rlim = []; 0084 T = 1; 0085 Wv = eye(k); 0086 Wu = eye(m); 0087 ud = zeros(m,1); 0088 0089 % Set values of submitted optional arguments 0090 for i=1:narg 0091 switch i 0092 case 1, rlim = varargin{i}; 0093 case 2, T = varargin{i}; 0094 case 3, Wv = varargin{i}; 0095 case 4, Wu = varargin{i}; 0096 case 5, ud = varargin{i}; 0097 end 0098 end 0099 0100 % Call generic control allocation simulation subroutine. 0101 [u,W,time,iter] = alloc_sim('qp',B,v,plim,rlim,T,'Wv',Wv,... 0102 'Wu',Wu,'ud',ud,varargin{iopt:end}); 0103