ip_alloc

 IP_ALLOC - Control allocation using interior point method.

  [u,iter] = ip_alloc(B,v,umin,umax,[ud,gamma,tol,imax])

 Solves the weighted, bounded least-squares problem

   min ||u-ud||^2 + gamma ||Bu-v||^2   (unit weighting matrices)

   subj. to  umin <= u <= umax

 using a primal dual interior point 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)
 ud    desired control (m x 1) [0]
 gamma weight (scalar) [1e4]
 tol   tolerance used in stopping criterion [1e-4]
 imax  max no. of iterations [100]
 
  Outputs:
  -------
 u     optimal control
 iter  no. of iterations

 See also: WLS_ALLOC, WLSC_ALLOC, FXP_ALLOC, QP_SIM.

 Contributed by John Petersen.

0001 function [u,iter] = ip_alloc(B,v,umin,umax,ud,gam,tol,imax)
0002   
0003 % IP_ALLOC - Control allocation using interior point method.
0004 %
0005 %  [u,iter] = ip_alloc(B,v,umin,umax,[ud,gamma,tol,imax])
0006 %
0007 % Solves the weighted, bounded least-squares problem
0008 %
0009 %   min ||u-ud||^2 + gamma ||Bu-v||^2   (unit weighting matrices)
0010 %
0011 %   subj. to  umin <= u <= umax
0012 %
0013 % using a primal dual interior point 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 % ud    desired control (m x 1) [0]
0022 % gamma weight (scalar) [1e4]
0023 % tol   tolerance used in stopping criterion [1e-4]
0024 % imax  max no. of iterations [100]
0025 %
0026 %  Outputs:
0027 %  -------
0028 % u     optimal control
0029 % iter  no. of iterations
0030 %
0031 % See also: WLS_ALLOC, WLSC_ALLOC, FXP_ALLOC, QP_SIM.
0032 %
0033 % Contributed by John Petersen.
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, tol = 1e-4; end
0040     if nargin < 6, gam = 1e4; end
0041     if nargin < 5, ud = zeros(m,1); end
0042   end
0043 
0044   % Reformulate   min  ||u-ud||^2 + gamma ||Bu-v||^2
0045   %               s.t. umin <= u <= umax
0046   %
0047   % as            min  ||Ax-b||^2 + h ||x-xd||^2
0048   %               s.t. 0 <= x <= xmax
0049   %
0050   % where x=u-umin, h=1/gamma, A=B, b=v-B*umin, xd=ud-umin, xmax=umax-umin
0051   h = 1/gam; A = B; b = v - B*umin; xd = ud - umin; xmax = umax - umin;
0052   
0053   % ||Ax-b||^2 + h ||x-xd||^2 = 1/2x'Hx + c'x + f(xd)
0054   c = -2*(b'*A + h*xd');
0055   
0056   % Solve QP problem.
0057   [x,iter] = pdq(A,b,c',xmax,h,tol,imax);
0058   
0059   % Optimal control.
0060   u = x + umin;
0061   
0062 function [x,iter] = pdq(A,b,c,u,wc,tol,imax);
0063   % Primal dual IP solver
0064   
0065   [k,m] = size(A);    % k = #constraints , m = #variables
0066   As2=A*sqrt(2);
0067   [s,w,x,z] = startpt(A,b,c,u,wc);
0068   rho = .9995; sig = 0.1; m2 = 2*m;
0069   xs = x.*s; wz = w.*z;
0070   mu = sig*(sum(xs + wz))/m2; % eq. (7)
0071   nxl=norm(x,1); iHd = 0; 
0072   ru = 0; rc = 0; rb = 0;
0073   
0074   for iter = 1:imax+1
0075     if (mu < tol) 
0076       % Close enough to optimum, bail out.
0077       break;
0078     end;   
0079     rxs = (xs - mu);
0080     iw = 1./w; rwz = (wz - mu);
0081     ix = 1./x; ixs = ix.*s;
0082     iwz = iw.*z;
0083     d = 2*wc + ixs + iwz;                         
0084     [ds,dw,dx,dz] = direct(d,As2,rb,rc,ru,rxs,rwz,ix,iw,ixs,iwz,z);
0085     alpha = stepsize(dx,ds,dw,dz,x,s,w,z);
0086     ralpha = rho*alpha;
0087     s = s + ralpha*ds;
0088     w = w + ralpha*dw;
0089     x = x + ralpha*dx;
0090     z = z + ralpha*dz;
0091 
0092     xs = x.*s; wz = w.*z;
0093     gap = sum(xs + wz)/m2;
0094     mu = min(.1,100*gap)*gap;
0095   
0096   end
0097   
0098   iter=iter-1;  %True number of iterations is one less
0099 
0100 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0101 % Initial starting point
0102 function [s,w,x,z] = startpt(A,b,c,u,wc);
0103   
0104   m = size(A,2);
0105   en = ones(m,1);
0106   % Start at the center of the constraint box
0107   x = u/2; w = u - x; z = en; s = en; 
0108   if 0 
0109     AA = csm(A');  % csm efficiently computes cb'*cb
0110     for i=1:m
0111       AA(i,i) = AA(i,i) + wc;
0112     end % H = 2(A'A+wcI);
0113   else
0114     AA = A'*A + wc*eye(m);
0115   end
0116   ec = c + 2*AA*x;  %initial residual error used to initialize z,s;
0117   g = .1;  sg = 1+g;
0118   i = find(ec>0); s(i) =  sg*ec(i); z(i) =  g*ec(i); % Hx + c + z - s = 0;
0119   i = find(ec<0); z(i) = -sg*ec(i); s(i) = -g*ec(i);
0120   
0121 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0122 % Compute step direction
0123 function [ds,dw,dx,dz] = direct(d,As2,rb,rc,ru,rxs,rwz,ix,iw,ixs,iwz,z);
0124   
0125   ixrxs = ix.*rxs; iwrwz = iw.*rwz;
0126   rr = iwrwz - ixrxs;
0127 
0128   if 0
0129     iHd = smwf(d,As2);  % smwf is a fast smw for the specific form used here
0130     dx = iHd*rr;
0131   else
0132     % iHd = inv(diag(d)+As2'*As2);
0133     dx = (diag(d)+As2'*As2)\rr;
0134   end
0135   ds = -ixs.*dx - ixrxs;    dw = -dx;
0136   dz = -iwz.*dw - iwrwz;
0137   
0138 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0139 % Compute stepsize
0140 function alpha = stepsize(dx,ds,dw,dz,x,s,w,z);
0141   i = find(ds<0); as = min(-s(i)./ds(i));    
0142   i = find(dw<0); aw = min(-w(i)./dw(i));    
0143   i = find(dx<0); ax = min(-x(i)./dx(i));    
0144   i = find(dz<0); az = min(-z(i)./dz(i));    
0145   alpha = min([aw ax as az 1]);    
0146 
0147   
0148   
0149   
0150   
0151   
0152 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0153 %  csm.m  compute symmetric matrix from X = B*B'
0154 function X = csm(B);
0155   
0156   [m,n] = size(B);
0157   for i=1:m-1
0158     for j=i+1:m
0159       X(i,j) = B(i,:)*B(j,:)';
0160       X(j,i) = X(i,j);
0161     end;    
0162   end;
0163   for i = 1:m;    X(i,i) = B(i,:)*B(i,:)'; end;
0164   
0165 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0166 % smwf.m  computes the inverse of (A + B'B) using Sherman-Morrison-Woodbury formula
0167 %    Fast version for specific matrices.
0168 %           where A is a diagonal matrix, but designated only by a vector
0169 %                                                    i.e. the input is the vector of the diagonal
0170 %                B is non-square matrices
0171 %   H = inv(A) - inv(A)B'*inv(B*inv(A)B' + I)*B*inv(A);
0172 %
0173 function         H = smwf(A,B);
0174   
0175   [k,m] = size(B);
0176   iA = 1./A; iAB = zeros(m,k); BiA = zeros(k,m);  BAB = zeros(k);
0177   for i = 1:k    iAB(:,i) = iA.*B(i,:)'; end;
0178   for i = 1:k        BiA(i,:) = B(i,:).*iA';    end;
0179   for i=1:k-1
0180     for j=i+1:k
0181       BAB(i,j) = B(i,:)*iAB(:,j);
0182       BAB(j,i) = BAB(i,j);
0183     end;
0184   end;
0185   for i = 1:k;    BAB(i,i) = B(i,:)*iAB(:,i); end;
0186   Q = BAB;
0187   for i = 1:k;    Q(i,i) = Q(i,i) + 1; end
0188   QBA = Q\iAB';
0189   for i=1:m-1
0190     for j=i+1:m
0191       ABQBA(i,j) = iAB(i,:)*QBA(:,j);
0192       ABQBA(j,i) = ABQBA(i,j);
0193     end;
0194   end;
0195   for i = 1:m;  ABQBA(i,i) = iAB(i,:)*QBA(:,i); end
0196   H = -ABQBA;
0197   for i = 1:m
0198     H(i,i) = iA(i) + H(i,i);
0199   end
0200