lseq_pen

 dir/udir ... new directions for x/equality constraints multipliers

 miter = v kolikate jsem velke iteraci, pokud 0 ~ delam prvni, dopad na rNu
 fx/gradx ~ function value/gradient of lagrangian, e.i., eqlty constraints included

0001 function [rRelStep, nFlag]=lseq_pen(obj, dir, udir, miter)
0002 % dir/udir ... new directions for x/equality constraints multipliers
0003 %
0004 % miter = v kolikate jsem velke iteraci, pokud 0 ~ delam prvni, dopad na rNu
0005 % fx/gradx ~ function value/gradient of lagrangian, e.i., eqlty constraints included
0006 %
0007 
0008 % TO DO:
0009 % * fx<HUGE_VAL ... check how it works
0010 % * bStop problem... (stop crit)
0011 % * fail-safe by using normal LS
0012 %
0013 
0014 %%%%%%%%% Settings %%%%%%%%%%
0015   rAlphaMin = 1.0e-14;
0016   rPrecfac=10.0;
0017   rMacheps=1.0e-16; 
0018 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0019 
0020   bJustStarted=0;
0021   if (isempty(obj.ls_rnu) || miter==0)
0022     obj.ls_rnu=0;
0023     bJustStarted=1;
0024   end
0025 
0026   LSEQ_MAX_ITER = obj.allopts.max_lseq_iter;
0027   xall0 = obj.xall;
0028   ueq0 = obj.ueq;
0029 
0030 %% co s multiplikatory?  global/parametr?
0031 %% Jak to brat defaultne? fx/gradx jsou Lagrangianu s multiplikatory? ano...?
0032 
0033   % find maximal step length
0034   rStepMax = 1.;
0035 
0036   % note: reduce gradient by eq. constr.
0037   rPi = dir'*(obj.ALdx - obj.eqdx*obj.ueq);
0038   rEqNorm0 = obj.eqx'*obj.eqx;
0039   rU0C0 = obj.ueq'*obj.eqx;
0040   rUgVh0 = (obj.ueq+udir)'*obj.eqx - 2*rU0C0;  %??
0041 
0042   % new penalty parameter rNu estimate
0043   rNu_old = obj.ls_rnu;
0044   if (rEqNorm0 > 1e-12)
0045     rEtadd = (rPi + rUgVh0) / rEqNorm0;
0046   else
0047     rEtadd = 0.;
0048   end
0049 
0050   if (bJustStarted)
0051     obj.ls_rnu = abs(rEtadd) + 0.1; % rNu0 = 0.1 !!!
0052   else
0053     obj.ls_rnu = max(0.,rEtadd) + 0.1;
0054   end
0055   %obj.ls_rnu = max(rNu_old, obj.ls_rnu);
0056 
0057   obj.print(4,Inf,'LSEQ (pen): penalty par. NU=%f', obj.ls_rnu);
0058       
0059   % Add new penalty term
0060   fx = obj.ALx;
0061   fx0 = fx;            % original name in line_search_eq.c: rFobj_old, fx~rFobj
0062   meritx0 = fx0 + .5*obj.ls_rnu*rEqNorm0;         % original name: rF0
0063   rDelal0 = rUgVh0 - obj.ls_rnu * rEqNorm0 + rPi;
0064 
0065   obj.print(4,Inf,'LSEQ (pen): NU: %e %e %f', rDelal0, rEqNorm0, rEtadd);
0066 
0067   rAlphaStep = 2.0*rStepMax;
0068 
0069   ok=0;
0070   for iter=1:LSEQ_MAX_ITER
0071     rAlphaStep = rAlphaStep / 2.0;        
0072         
0073     % Compute new trial point and evaluate objective and constraint
0074     % function at that point
0075     obj.xall = xall0 + rAlphaStep*dir;
0076     obj.ueq = ueq0 + rAlphaStep*udir;
0077       
0078     obj.eval_alx();
0079     fx=obj.ALx;
0080     %[fx,eqx]=fnc.obj(x_it,ueq_it);
0081     rEqNorm=obj.eqx'*obj.eqx;
0082     meritx = fx + .5*obj.ls_rnu*rEqNorm;
0083 
0084     if (iter==1 && abs((fx - fx0)/(.5*(fx + fx0))) + abs(rEqNorm0 - rEqNorm) < 1.0e-14)
0085       ok=2;    % in Pennlp: bStop <-- 1; ~ stop crit of unconstr_min <d,g>
0086       break;
0087     end
0088 
0089     rUC = obj.ueq'*obj.eqx;
0090     rUgVh = rUC - rU0C0;
0091 
0092     rZlhs = fx - fx0 + rUgVh - 1.0e-4 * rAlphaStep * (rPi + rUgVh0);
0093     rZrhs = (rEqNorm0 - rEqNorm) * .5 - rAlphaStep * 1.0e-4 * rEqNorm0;
0094 
0095     if (abs(rZrhs) <= 1.0e-15 && rZlhs <= 1.0e-15 && isfinite(fx)) % && *fX < HUGE_VAL)
0096       ok=1;
0097       break;
0098     end
0099     if (rZrhs >= 0. && isfinite(fx)) % && *fX < HUGE_VAL)
0100       ok=1;
0101       break;
0102     end
0103         
0104     rNu_ls = rZlhs / rZrhs;
0105 
0106     if (iter > 1 && rNu_ls > obj.ls_rnu && isfinite(fx)) % && *fX < HUGE_VAL)
0107       ok=1;
0108       break;
0109     end
0110 
0111     rLhs = meritx - meritx0 - rPrecfac * rMacheps * abs(meritx0);
0112     rRhs = rAlphaStep * 1.0e-4 * rDelal0;
0113         
0114     if (rLhs <= rRhs && isfinite(fx)) % && *fX < HUGE_VAL)
0115       ok=1;
0116       break;
0117     end 
0118 
0119     if (rAlphaStep <= rAlphaMin)   % really????
0120       ok=1;
0121       break;
0122     end 
0123 
0124   end
0125 
0126   % If evrth. OK update ...
0127   if (ok>0)
0128     obj.print(3,Inf,'LSEQ (pen): %i steps, rel. width %f',iter,rAlphaStep);
0129     %x=x_it;
0130     %ueq=ueq_it;
0131     obj.eval_aldx();
0132     %[gradx, gradeqx] = fnc.obj_grad(x, ueq);
0133     rRelStep=rAlphaStep;
0134     nFlag=0;
0135   else
0136     obj.print(3,Inf,'LSEQ (pen): failed - step too short, max_ls_it (%i)',iter);
0137     % x/ueq unchanged
0138     obj.xall=xall0;
0139     obj.ueq=ueq0;
0140     obj.eval_alx();
0141     %[fx,eqx]=fnc.obj(x,ueq);
0142     rRelStep=0;
0143     nFlag=3;
0144   end
0145 
0146   obj.lsiter_last = obj.lsiter_last+iter;
0147 
0148 %  if (!ok)
0149 %    for iter=1:4
0150 %      obj.ls_rnu = obj.ls_rnu * 10.0;
0151 %      obj.print(3,Inf,'LSEQ (pen): Nu = %10.2E (short step)', obj.ls_rnu);
0152 %      merit_obj=@(xtmp,ueqtmp) add_penalty_to(fnc.obj,obj.ls_rnu,xtmp,ueqtmp);
0153        % call ordinary linesearch, do not forget, that now it returns values of the merit function!
0154 %      rRelStepLength = line_search((* objective), (* gradient_obj), gradx, S, X, fX, rStepLength, 1., 1);
0155 %      if ok ... break;
0156 %    end
0157 %  end
0158 
0159   return;
0160 
0161 
0162 function [phi, constr] = add_penalty_to(objective, rNu, x, ueq)
0163 % suppose [f, constr]=objective(x,ueq), create a new function (merit function)
0164 % such that returns [f+0.5*rNu*constr'*constr]
0165 % need to mimic behaviour as objective but need to return merit function
0166 % usage: merit=@(xtmp,utmp) add_penalty_to(obj, rNu, xtmp, utmp)
0167 % then merit(x,ueq) does its job
0168 
0169   % this would need to be changed, but it is not used anyway
0170   error('this is not adapted for PenLab');
0171   [f, constr]=objective(x,ueq);
0172   phi = f + 0.5*rNu*(constr'*constr);
0173 
0174   return;
0175