CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Jun 2009 C C Dec 2009: An upper bound on number of lower bound constraints is added C C Dec 2010: A bug on second round lb heuristics fixed C C C This Fortran code solves approximately the sensor localization problem C min_{x_1,...,x_m} |||xi-xj||^2 - dij^2 |, C by approximating it with a robust ESDP relaxation and solving this relaxation with C a log-barrier penalty coordinate descent method. Specifically, a block-coordinate C gradient descent method is used to solve the log-barrier problem C min sum_{i0 is decreased periodically. C In the code, we set rho_{ij}=(d_{ij}^2)*(1/(1-2*sigma)^2-1). C The n points are assumed to be in R^2, with the first m points being sensors and C the remaining n-m points being anchors. C C This version has a preprocessing phase that prunes the edges to maintain the number C of neighboring sensors below a user-specified threshold. C There is also a user-specified option to add edges back as needed. C This version has two optional postprocessing phases that refine the sensor positions. C C The first part of the postprocessing phase refines the solution from the previous stage C by applying a block-coordinate gradient descent method to solve the log-barrier problem C min sum_{i0 is decreased periodically. C Sensors whose individual traces are smaller than a prescribed value are treated as anchors C in this stage. Users can specify the percentage of violated range constraints to add. C C The second part of the postprocessing phase refines the solution from the previous stages by C applying block-coordinate gradient descent to locally optimize the function C C sum_{(i,j)}(||xi-xj||-dij)^2 C C The user can choose whether to hold fixed those sensors whose individual traces C are below a user-specified value during this refinement phase. C C Problem Data (needed for the input): C n number of points (with anchors at the end) C na number of anchors C dim 2 C nd number of edges C head(k) head of the kth edge, k=1,...,nd C tail(k) tail of the kth edge, k=1,...,nd C head2(k) head of the kth lb-info, k=1,...,ndd C tail2(k) tail of the kth lb-info, k=1,...,ndd C d(k) measured distance for the kth edge, k=1,...,nd C r(k) estimated lower bound of range for kth lb-info, k=1...,ndd C sol(1,i) initial 1st coordinate of the ith sensor, i=1,...,m, C sol(2,i) initial 2nd coordinate of the ith sensor, i=1,...,m C C Algorithm Parameters: C sigma Estimate of the relative noise standard deviation. sigma must be nonnegative. C degbd Maximum number of neighboring sensors. C degbd should be a positive integer (e.g., 5 or 10 or m). C Addbk 1 if deleted edges are to be added back as needed (when mu is decreased); C 0 else. C Refine 1 if all sensors are refined in the postprocessing; C 0 if only those sensor with large trace are refined. C mu_init Initial value for mu (default is 10D0). C mu_final Final value for mu (default is 1d-14). C factor Amount by which mu is decreased. factor must be > 1 (default is 10). C tol Lower limit on the tolerance on the gradient with respect to C Z_i = (x_i,y_{ii},y_{ij}) (default is 1d-7). C C Output: C The code displays the total run time and outputs the robust ESDP solution C x_i, y_{ii} in the ascii file sensorsoln.dat. C The solution x_i, y_{ii} after the first refinement stage is stored in sensorsoln3.dat. C If this solution is further refined in the postprocessing, the refined solution C x_i is saved in the ascii file sensorsoln2.dat. C C Reference: C Ting Kei Pong, Edge-based semidefinite programming relaxation of sensor network localization C with lower bound constraints. Report, Dept. Mathematics, Univ. Washington, Seattle, Dec 2009. C C Authors: Ting Kei Pong. Questions about the code may be directed to him. C C The routines are modified largely based on sensor_esdp.f, coauthored with Paul Tseng. C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC PARAMETER (MAXN=10000, MAXA=400000, MAXDEG=203) INTEGER head(MAXA),tail(MAXA),done(MAXN),deg(MAXN) INTEGER FIN(MAXN),FOU(MAXN),NXTIN(MAXA),NXTOU(MAXA) INTEGER head2(MAXA),tail2(MAXA),ndone,nquit,niter,degbd,rgebd INTEGER FIN2(MAXN),FOU2(MAXN),NXTIN2(MAXA),NXTOU2(MAXA) INTEGER n,na,m,dim,nd,nnb,ndd INTEGER Refine,Addbk,Addbk2,unfix REAL*8 sigma,tol,mu_ini,mu_fin,factor REAL*8 mu,toll,ddd,OBJ,obji,rrr REAL*8 sol(2,MAXN),d(MAXA),x(2,MAXN),y(MAXN),r(MAXA),tr(MAXN) REAL*8 y2(MAXA),dd(MAXA),rho(MAXA),gyi,y3(MAXA),rr(MAXA) REAL*8 gxi(2),gy2(MAXDEG),normg2,tmp0,tmp1,tmp,rr2 REAL*8 tii,tjj,tij,newxi(2),newyi,newyij,dderiv,step REAL*8 dm,nobji,oldobj,truOBJ REAL*8 tx1,tx2,tii2,meps,xi(2),Lxy(3,maxdeg) REAL*8 H(6),Hy(maxdeg),Hxy(3,maxdeg),L(6),Ly(maxdeg) REAL*8 w(maxdeg),dir(maxdeg),w1,w2,w3,d1,d2,d3,a,c,LS(6) INTEGER y4(MAXA), y5(MAXA), m2,s REAL*8 obj2,obj2i,nobj2i,tr_tol(MAXN),infty,dis,trtol,eps REAL*8 ftr,error INTEGER iarray(MAXA),count REAL*8 array(MAXA), lgth(MAXA),itmp,IMAX,prcnt, reg real etime, tarr1(2),TT,T3,TT3,tarr3(2),timer,tarr2(2),T2 external etime C diagnostic variables C real*8 z(maxdeg),z1,z2,z3,err,yj,x1i,x2i,x1j,x2j C Set machine epsilon meps=1d-40 CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Read in n na dim nd head tail d x C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC PRINT*,'READING PROBLEM DATA' OPEN(13,FILE='sensorprob.dat',STATUS='OLD') REWIND(13) READ(13,1000) n,na,dim,nd,ndd m=n-na PRINT*,' #pts=',n,' #anchors=',na,' #sensors=',m,' #edges=',nd, $ ' #rge constraint=',ndd DO 1 I=1,nd READ(13,1010) head(I),tail(I),d(I) 1 CONTINUE DO 2 I=1,n READ(13,1020) sol(1,I),sol(2,I) 2 CONTINUE DO 3 I=1,ndd READ(13,1010) head2(I),tail2(I),r(I) 3 CONTINUE CLOSE(13) 1000 FORMAT(5I8) 1010 FORMAT(2I8,F12.8) 1020 FORMAT(2F12.8) 1021 FORMAT(3F12.8) CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Enter algorithm parameters C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC PRINT*,' enter estimate of relative distance noise ' print*,' (e.g., 0 for no noise, 0.1 for 10% noise) ' READ*, sigma PRINT*,' enter upper bd on #sensor neighbors ' READ*, degbd PRINT*,' enter upper bd on range constraints' READ*, rgebd C PRINT*,' enter 1 to add back some deleted edges' C READ*, Addbk Addbk = 0 C PRINT*,'enter termination tolerance for gradient (1d-7)' C READ*, tol tol=1d-7 C final mu C PRINT*,' enter initial mu (10)' C READ*, mu_ini C PRINT*,' enter final mu (1d-14)' C READ*, mu_fin mu_ini=1d-1 mu_fin=1d-14 C initialize mu mu=mu_ini factor=10d0 PRINT*,' this code seeks the analy. center soln described' CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Initialize timing routine C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC TIMER = etime(tarr1) TIMER = tarr1(1) niter=0 CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Build linked list for neighborhood structure in network C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C Initial estimate of sensor locations. DO 10 i=1,n x(1,i)=sol(1,i) x(2,i)=sol(2,i) 10 CONTINUE tmp=-1.D0+1.D0/(1.D0-2.d0*sigma)**2 DO 11 k=1,nd rho(k)=tmp*(d(k)**2) 11 CONTINUE DO 12 k=1,ndd r(k)=(r(k))**2 12 CONTINUE C Initialize y(i,i) DO 13 i=1,m y(i)=x(1,i)**2+x(2,i)**2+1D0 13 CONTINUE C Set initial gradient tolerance be mu toll=mu C Build linked list for neighborhood structure in network DO 14 I=1,m FIN(I)=0 FOU(I)=0 C deg(i) will count the #sensor neighbors of sensor i deg(i)=0 14 CONTINUE DO 15 k=1,nd i=head(k) j=tail(k) C y4 will record which edge is dropped; y4(k)=1 if it is dropped y4(k)=0 C swap head and tail IF (i.GT.m) THEN head(k)=j tail(k)=i i=j j=tail(k) ENDIF C Drop edges to keep each sensor to have at most degbd nhbr sensors IF (j.LE.m) THEN IF ((deg(i).lt.degbd).and.(deg(j).lt.degbd)) THEN deg(i)=deg(i)+1 deg(j)=deg(j)+1 nxtou(k)=fou(i) fou(i)=k nxtin(k)=fin(j) fin(j)=k C initialize y(i,j) y2(k)=x(1,i)*x(1,j)+x(2,i)*x(2,j) ELSE y4(k)=1 ENDIF ELSE nxtou(k)=fou(i) fou(i)=k ENDIF 15 CONTINUE CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Loop for approaching the central path C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC DO WHILE (mu.GE.mu_fin) OBJ=0.D0 DO 17 i=1,m tii=y(i)-x(1,i)**2-x(2,i)**2 k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF (j.LE.m) THEN dd(k)=y(i)-2D0*y2(k)+y(j)-d(k)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=y2(k)-x(1,i)*x(1,j)-x(2,i)*x(2,j) dm=tii*tjj-tij**2 OBJ=OBJ-mu*DLOG(dm) ELSE dd(k)=y(i)-2D0*x(1,i)*x(1,j)-2D0*x(2,i)*x(2,j)+ $ x(1,j)**2+x(2,j)**2-d(k)**2 ENDIF tmp=(DMAX1(0.D0,dd(k)-rho(k)))**2+ $ (DMAX1(0.D0,-dd(k)-rho(k)))**2 OBJ=OBJ+tmp/2D0 k=nxtou(k) END DO OBJ=OBJ-mu*DLOG(y(i)-x(1,i)**2-x(2,i)**2) 17 CONTINUE ndone=0 nquit=0 C done(i) = 0 if i needs to be iterated DO 18 i=1,m done(i)=0 18 CONTINUE C enters the loop for LPCGD DO WHILE (ndone+nquit.LT.m) ndone=0 nquit=0 DO 20 i=1,m IF (done(i).EQ.0) THEN niter=niter+1 CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C block-coordinate gradient descent iteration for sensor i C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC gxi(1)=0.D0 gxi(2)=0.D0 gyi=0.D0 obji=0.D0 normg2=0.D0 nnb=0 do 25 k=1,6 25 H(k)=0.d0 xi(1)=x(1,i) xi(2)=x(2,i) tii=y(i)-xi(1)**2-xi(2)**2 tii2=tii**2 k=fou(i) DO WHILE (k.GT.0) j=tail(k) tmp0=DMAX1(0.D0,dd(k)-rho(k)) tmp1=DMAX1(0.D0,-dd(k)-rho(k)) tmp=tmp0-tmp1 obji=obji+tmp0**2+tmp1**2 gyi=gyi+tmp IF (j.GT.m) THEN gxi(1)=gxi(1)-2D0*tmp*x(1,j) gxi(2)=gxi(2)-2D0*tmp*x(2,j) if (tmp0+tmp1.gt.0) then c Add quadratic-penalty Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+4D0*x(1,j)**2 H(2)=H(2)+4D0*x(1,j)*x(2,j) H(3)=H(3)-2D0*x(1,j) H(4)=H(4)+4D0*x(2,j)**2 H(5)=H(5)-2D0*x(2,j) H(6)=H(6)+1D0 end if ELSE C Construct the entries of Y-X'*X corresponding to i and j tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=y2(k)-xi(1)*x(1,j)-xi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 1a: ',i,k,j,dm pause end if obji=obji-2D0*mu*DLOG(dm) tx1=tij*x(1,j)-tjj*xi(1) tx2=tij*x(2,j)-tjj*xi(2) gxi(1)=gxi(1)-2D0*mu*tx1/dm gxi(2)=gxi(2)-2D0*mu*tx2/dm gyi=gyi-mu*tjj/dm nnb=nnb+1 gy2(nnb)=(-2D0)*tmp+2D0*mu*tij/dm normg2=normg2+gy2(nnb)**2 c Log-barrier Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+mu*(2D0*(tjj+x(1,j)**2)+4D0*tx1**2/dm)/dm H(2)=H(2)+mu*(2D0*x(1,j)*x(2,j)+4D0*tx1*tx2/dm)/dm H(3)=H(3)+mu*2D0*tjj*tx1/dm**2 H(4)=H(4)+mu*(2D0*(tjj+x(2,j)**2)+4D0*tx2**2/dm)/dm H(5)=H(5)+mu*2D0*tjj*tx2/dm**2 H(6)=H(6)+mu*(tjj/dm)**2 C Log-barrier Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=mu*(2D0+4D0*tij**2/dm)/dm Hxy(1,nnb)=mu*((-2D0)*x(1,j)-4D0*tij*tx1/dm)/dm Hxy(2,nnb)=mu*((-2D0)*x(2,j)-4D0*tij*tx2/dm)/dm Hxy(3,nnb)=(-mu)*2D0*tij*tjj/dm**2 if (tmp0+tmp1.gt.0.D0) then C Add quadratic-penalty Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=Hy(nnb)+4D0 Hxy(3,nnb)=Hxy(3,nnb)-2D0 H(6)=H(6)+1D0 end if END IF k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) tmp0=DMAX1(0.D0,dd(k)-rho(k)) tmp1=DMAX1(0.D0,-dd(k)-rho(k)) tmp=tmp0-tmp1 obji=obji+tmp0**2+tmp1**2 gyi=gyi+tmp C Construct the entries of Y-X'*X corresponding to i and j tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=y2(k)-xi(1)*x(1,j)-xi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 1b: ',i,k,j,dm pause end if obji=obji-2D0*mu*DLOG(dm) tx1=tij*x(1,j)-tjj*xi(1) tx2=tij*x(2,j)-tjj*xi(2) gxi(1)=gxi(1)-2D0*mu*tx1/dm gxi(2)=gxi(2)-2D0*mu*tx2/dm gyi=gyi-mu*tjj/dm nnb=nnb+1 gy2(nnb)=(-2D0)*tmp+2D0*mu*tij/dm normg2=normg2+gy2(nnb)**2 c Log-barrier Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+mu*(2D0*(tjj+x(1,j)**2)+4D0*tx1**2/dm)/dm H(2)=H(2)+mu*(2D0*x(1,j)*x(2,j)+4D0*tx1*tx2/dm)/dm H(3)=H(3)+mu*2D0*tjj*tx1/dm**2 H(4)=H(4)+mu*(2D0*(tjj+x(2,j)**2)+4D0*tx2**2/dm)/dm H(5)=H(5)+mu*2D0*tjj*tx2/dm**2 H(6)=H(6)+mu*(tjj/dm)**2 C Log-barrier Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=mu*(2D0+4D0*tij**2/dm)/dm Hxy(1,nnb)=mu*((-2D0)*x(1,j)-4D0*tij*tx1/dm)/dm Hxy(2,nnb)=mu*((-2D0)*x(2,j)-4D0*tij*tx2/dm)/dm Hxy(3,nnb)=(-mu)*2D0*tij*tjj/dm**2 if (tmp0+tmp1.gt.0.D0) then C Add quadratic-penalty Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=Hy(nnb)+4D0 Hxy(3,nnb)=Hxy(3,nnb)-2D0 H(6)=H(6)+1D0 end if k=nxtin(k) END DO c Add 2nd log-barrier Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+mu*(2D0+4D0*xi(1)**2/tii)/tii H(2)=H(2)+mu*4D0*xi(1)*xi(2)/tii2 H(3)=H(3)-mu*2D0*xi(1)/tii2 H(4)=H(4)+mu*(2D0+4D0*xi(2)**2/tii)/tii H(5)=H(5)-mu*2D0*xi(2)/tii2 H(6)=H(6)+mu/tii2 gyi=gyi-mu/tii gxi(1)=gxi(1)+2D0*mu*xi(1)/tii gxi(2)=gxi(2)+2D0*mu*xi(2)/tii normg2=DSQRT(normg2+gxi(1)**2+gxi(2)**2+gyi**2) obji=obji/2D0 obji=obji-mu*DLOG(tii) CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Check stopping criterion C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC IF (normg2.LT.toll) THEN done(i)=1 ndone=ndone+1 ELSE CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Cholesky factorization of Hessian (arrow sparsity) H=L*L' C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC do 30 k=1,6 30 LS(k)=0.d0 do 31 k=1,nnb Ly(k)=dsqrt(Hy(k)) Lxy(1,k)=Hxy(1,k)/Ly(k) Lxy(2,k)=Hxy(2,k)/Ly(k) Lxy(3,k)=Hxy(3,k)/Ly(k) LS(1)=LS(1)+Lxy(1,k)**2 LS(2)=LS(2)+Lxy(1,k)*Lxy(2,k) LS(3)=LS(3)+Lxy(1,k)*Lxy(3,k) LS(4)=LS(4)+Lxy(2,k)**2 LS(5)=LS(5)+Lxy(2,k)*Lxy(3,k) LS(6)=LS(6)+Lxy(3,k)**2 31 continue if (H(1).le.LS(1)) then print*,i,1,H(1)-LS(1) pause nquit=nquit+1 GO TO 20 end if L(1)=dsqrt(H(1)-LS(1)) L(2)=(H(2)-LS(2))/L(1) L(3)=(H(3)-LS(3))/L(1) if (H(4).le.LS(4)+L(2)**2) then print*,i,4,H(4)-LS(4)-L(2)**2 pause nquit=nquit+1 GO TO 20 end if L(4)=dsqrt(H(4)-LS(4)-L(2)**2) L(5)=(H(5)-LS(5)-L(2)*L(3))/L(4) if (H(6).le.LS(6)+L(3)**2+L(5)**2) then print*,i,6,H(6)-LS(6)-L(3)**2-L(5)**2 pause nquit=nquit+1 GO TO 20 end if L(6)=dsqrt(H(6)-LS(6)-L(3)**2-L(5)**2) C Solve L*w=-g for w w1=-gxi(1) w2=-gxi(2) w3=-gyi do 35 k=1,nnb w(k)=(-gy2(k))/Ly(k) w1=w1-Lxy(1,k)*w(k) w2=w2-Lxy(2,k)*w(k) w3=w3-Lxy(3,k)*w(k) 35 continue w1=w1/L(1) w2=(w2-L(2)*w1)/L(4) w3=(w3-L(3)*w1-L(5)*w2)/L(6) C Solve L'*d=w for descent direction dir & compute dir'*g d3=w3/L(6) d2=(w2-L(5)*d3)/L(4) d1=(w1-L(2)*d2-L(3)*d3)/L(1) dderiv=d1*gxi(1)+d2*gxi(2)+d3*gyi do 40 k=nnb,1,-1 dir(k)=(w(k)-Lxy(1,k)*d1-Lxy(2,k)*d2- $ Lxy(3,k)*d3)/Ly(k) dderiv=dderiv+gy2(k)*dir(k) 40 continue CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Initialize the stepsize in Armijo line search along dir C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC oldobj=obj C Initialize stepsize so that it's below 1 & y_ii^new > ||x_i^new||^2 a=d1**2+d2**2 c=d3-2D0*(x(1,i)*d1+x(2,i)*d2) if (c.ge.0.D0) then step=(c+dsqrt(c**2+4d0*a*tii))/(2D0*a)*0.99D0 else step=2D0*tii/(dsqrt(c**2+4d0*a*tii)-c)*0.99D0 endif if (step.gt.1.D0) then step=1.D0 end if newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 c print*,'dderiv,step,tr_i(Ztrial),a,c = ',dderiv,step,tii,a,c C Decrease step if not feasible for all nbhr j sequentially. (This C does NOT ensure the stepsize is feasible, but it's only one pass and C yields a feasible stepsize most of the times.) nnb=0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF (j.LE.m) THEN nnb=nnb+1 newyij=y2(k)+step*dir(nnb) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 end if END IF k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) nnb=nnb+1 newyij=y2(k)+step*dir(nnb) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 end if k=nxtin(k) END DO C Evaluate the part of the new objective value involving y_i, x_i, y_{ij} C & decrease step further if not feasible for all nbhr j simultaneously. C This ensures the stepsize is feasible, though multiple passes may be C needed occasionally. 50 nobji=0.d0 nnb=0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF (j.GT.m) THEN ddd=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)*x(1,j)+x(2,j)*x(2,j)-d(k)**2 ELSE nnb=nnb+1 newyij=y2(k)+step*dir(nnb) ddd=newyi-2D0*newyij+y(j)-d(k)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then step=0.75D0*step newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 go to 50 end if nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji+(DMAX1(0.D0,ddd-rho(k)))**2+ $ (DMAX1(0.D0,-ddd-rho(k)))**2 k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) nnb=nnb+1 newyij=y2(k)+step*dir(nnb) ddd=newyi-2D0*newyij+y(j)-d(k)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then step=0.75D0*step newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 go to 50 end if nobji=nobji-2D0*mu*DLOG(dm) nobji=nobji+(DMAX1(0.D0,ddd-rho(k)))**2 $ +(DMAX1(0.D0,-ddd-rho(k)))**2 k=nxtin(k) END DO nobji=nobji/2D0 nobji=nobji-mu*DLOG(tii) c print*,' feas. step= ',step,' new obj= ',nobji,' old obj= ',obji CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Armijo rule C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC DO WHILE (((nobji-obji).GT.(0.1D0*step*dderiv)) $ .AND.(step.GT.1d-5)) 60 step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 C Evaluate the part of the new objective value involving y_{ii}, x_i, y_{ij} nobji=0.d0 nnb=0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF (j.GT.m) THEN ddd=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)**2+x(2,j)**2-d(k)**2 ELSE nnb=nnb+1 newyij=y2(k)+step*dir(nnb) ddd=newyi-2D0*newyij+y(j)-d(k)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 3a: ',i,k,j,dm pause go to 60 end if nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji+(DMAX1(0.D0,ddd-rho(k)))**2+ $ (DMAX1(0.D0,-ddd-rho(k)))**2 k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) nnb=nnb+1 newyij=y2(k)+step*dir(nnb) ddd=newyi-2D0*newyij+y(j)-d(k)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 3b: ',i,k,j,dm pause go to 60 end if nobji=nobji-2D0*mu*DLOG(dm) nobji=nobji+(DMAX1(0.D0,ddd-rho(k)))**2 $ +(DMAX1(0.D0,-ddd-rho(k)))**2 k=nxtin(k) END DO nobji=nobji/2D0 nobji=nobji-mu*DLOG(tii) END DO IF (step.LT.1d-5) THEN C Can choose a bit larger than 1d-5 nquit=nquit+1 GO TO 20 ELSE IF (nobji.GT.obji) THEN PRINT*,'objective value increases' PRINT*,'Press to continue' pause nquit=nquit+1 GO TO 20 END IF C Accept the current stepsize and the new y_{ii}, x_i, y_{ij} obj=oldobj+nobji-obji x(1,i)=newxi(1) x(2,i)=newxi(2) y(i)=newyi nnb=0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF (j.GT.m) THEN dd(k)=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2) $ *x(2,j)+x(1,j)**2+x(2,j)**2-d(k)**2 ELSE nnb=nnb+1 y2(k)=y2(k)+step*dir(nnb) dd(k)=newyi-2D0*y2(k)+y(j)-d(k)**2 done(j)=0 END IF k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) nnb=nnb+1 y2(k)=y2(k)+step*dir(nnb) dd(k)=newyi-2D0*y2(k)+y(j)-d(k)**2 done(j)=0 k=nxtin(k) END DO if (nobji-obji.ge.(-mu_fin)*(dabs(obji)+1D0)) then c print*,'tiny change in obj! ' c print*,(nobji-obji)/(dabs(obji)+1D0) c pause nquit=nquit+1 end if END IF END IF ELSE ndone=ndone+1 END IF C print*, ' mu,iter,ndone,obj=',mu,niter,step,obj C pause 20 CONTINUE C PRINT*,' mu, iter, #not done, obj, step, nquit', C $ mu, niter, m-ndone, obj, step, nquit END DO C Restore dropped edges having positive cost IF (Addbk.EQ.1) THEN DO 70 k=1,nd i=head(k) j=tail(k) IF (j.LE.m) THEN IF (y4(k).eq.1) THEN C Choose yij to make the cost zero. Does this satisfy the psd constraint? tii=y(i)-x(1,i)**2-x(2,i)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tmp=(y(i)+y(j)-d(k)**2)/2D0 tij=tmp-x(1,i)*x(1,j)-x(2,i)*x(2,j) dm=tii*tjj-tij**2 IF (dm.le.0.D0) THEN C The psd constraint is not satisfied. Add back the edge. nxtou(k)=fou(i) fou(i)=k nxtin(k)=fin(j) fin(j)=k C initialize y(i,j) y2(k)=x(1,i)*x(1,j)+x(2,i)*x(2,j) y4(k)=0 ENDIF ENDIF ENDIF 70 CONTINUE ENDIF mu=mu/factor IF (toll.GT.tol) THEN toll=toll/factor ENDIF END DO CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C END OF THE FIRST PART C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC TT = etime(tarr1) TT = tarr1(1)-TIMER truOBJ=0.D0 DO 510 k=1,nd j=tail(k) IF (j.LE.m) THEN IF (y4(k).eq.1) THEN i=head(k) C Choose yij to make the cost zero. Does this satisfy the psd constraint? tii=y(i)-x(1,i)**2-x(2,i)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tmp0=(y(i)+y(j)-d(k)**2)/2D0 tmp1=x(1,i)*x(1,j)+x(2,i)*x(2,j) tij=tmp0-tmp1 dm=tii*tjj-tij**2 IF (dm.lt.0) THEN C set y(i,j) to minimize the ESDP cost while satisfying the psd constraints y2(k)=dsign(dsqrt(tii*tjj),tmp1-tmp0)+tmp1 dd(k)=y(i)-2D0*y2(k)+y(j)-d(k)**2 ELSE y2(k)=tmp0 dd(k)=0d0 ENDIF ENDIF ENDIF tmp=(DMAX1(0.D0,dd(k)-rho(k)))**2+ $ (DMAX1(0.D0,-dd(k)-rho(k)))**2 truOBJ=truOBJ+tmp 510 CONTINUE truOBJ=truOBJ/2D0 PRINT*,'1st PART = ',TT, ' secs.' PRINT*,' rho-ESDP obj= ',OBJ,' true rho-ESDP obj (all edges)= ', $truOBJ PRINT*,' iter= ',niter,' nquit= ',nquit DO 620 i=1,m sol(1,i)=x(1,i) sol(2,i)=x(2,i) tr(i)=y(i)-x(1,i)**2-x(2,i)**2 620 CONTINUE OPEN(12,FILE='sensorsoln.dat',STATUS='NEW') REWIND(12) DO 630 i=1,m WRITE(12,1021) sol(1,I), sol(2,I), tr(I) 630 CONTINUE CLOSE(12) PRINT*,'Done writing file' CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C C The second loop of LPCGD, on reduced network by fixing accurately positioned sensors C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC PRINT*,'Enter 1 to check lb constraints, 2 to skip' READ*,Refine IF (.NOT.(Refine.EQ.1)) THEN GO TO 179 ENDIF C Compute individual traces tmp=1d-2+30D0*sigma DO 590 I=1,m s=0 dis=0.D0 k=fou(i) DO WHILE (k.GT.0) s=s+1 dis=dis+d(k) k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) s=s+1 dis=dis+d(k) k=nxtin(k) END DO IF (s.GT.0) THEN dis=(dis/s)**2 ELSE dis=0.D0 ENDIF tr_tol(i)=dis*tmp 590 CONTINUE C Set initial gradient tolerance to be mu 1779 toll=mu_ini mu=toll C Count number of fixed sensors s=0 DO 513 i=1,m IF (tr(i).LE.tr_tol(i)) THEN s=s+1 ENDIF 513 CONTINUE PRINT*, ' Number of fixed sensors = ', s unfix=m-s PRINT*, ' Number of unfixed sensors = ', unfix DO 514 i=m+1,n tr(i)=0.D0 tr_tol(i)=0.D0 514 CONTINUE C Count number of violated lb constraints s=0 error=0.D0 DO 16 k=1,ndd i=head2(k) j=tail2(k) C y5 will record which "edge" is dropped; y5=1 means that "edge" is dropped y5(k)=1 C swap head and tail IF (i.GT.m) THEN head2(k)=j tail2(k)=i i=j j=tail2(k) ENDIF IF ((tr(i).GT.tr_tol(i)).OR.(tr(j).GT.tr_tol(j))) THEN lgth(k)=(x(1,i)-x(1,j))**2+(x(2,i)-x(2,j))**2-r(k) IF (lgth(k).LE.0.D0) THEN s=s+1 array(s)=DABS(lgth(k)) error=error+array(s) iarray(s)=k ENDIF ENDIF 16 CONTINUE PRINT*, ' Violated lb constraints = ', s PRINT*, ' Range error = ', error PRINT*, ' Enter 1 to add lb constraints, 2 otherwise' READ*, Refine IF (.NOT.(Refine.EQ.1)) THEN GO TO 179 ENDIF PRINT*,'Enter % of constraints to add (type 20 for 20 %)' READ*, prcnt C PRINT*, 'Enter upper bound on #sensor neighbors' C READ*, degbd C Initialize y(i,i) and fin2, fou2 DO 150 i=1,m FIN2(i)=0 FOU2(i)=0 IF (tr(i).GT.tr_tol(i)) THEN y(i)=x(1,i)**2+x(2,i)**2+1D0 ENDIF 150 CONTINUE DO 151 i=1,n deg(i)=0 151 CONTINUE C Bubble sort to find most violated lb constraints infty=1d40 IMAX=s-1 DO 213 I=1,s-1 tmp=infty DO 123 J=1,IMAX IF(array(J).GT.array(J+1)) GO TO 123 tmp=array(J) array(J)=array(J+1) array(J+1)=tmp itmp=iarray(J) iarray(J)=iarray(J+1) iarray(J+1)=itmp 123 CONTINUE IF(tmp.EQ.infty) GO TO 321 IMAX=IMAX-1 213 CONTINUE C compute the number of lb constraints wanted 321 s=-MAX1(REAL(-NINT(s*prcnt/100)),REAL(-unfix)) tmp=0 DO 1997 count=1,s k=iarray(count) i=head2(k) j=tail2(k) IF ((deg(i).lt.rgebd).and.(deg(j).lt.rgebd)) THEN tmp=tmp+1 deg(i)=deg(i)+1 deg(j)=deg(j)+1 nxtou2(k)=fou2(i) fou2(i)=k nxtin2(k)=fin2(j) fin2(j)=k C Add this lb constraint y5(k)=0 ENDIF 1997 CONTINUE PRINT*, ' Used lb constraints = ', tmp PRINT*, ' enter regularization parameter for lb constraints' READ*, reg C Build linked list for neighborhood structure in network DO 1914 I=1,m FIN(I)=0 FOU(I)=0 C deg(i) will count the #sensor neighbors of sensor i deg(i)=0 1914 CONTINUE DO 1915 k=1,nd i=head(k) j=tail(k) C y4 will record which edge is dropped; y4(k)=1 if it is dropped y4(k)=0 C swap head and tail IF (i.GT.m) THEN head(k)=j tail(k)=i i=j j=tail(k) ENDIF C Drop edges to keep each sensor to have at most degbd nhbr sensors IF (j.LE.m) THEN IF ((deg(i).lt.degbd).and.(deg(j).lt.degbd)) THEN deg(i)=deg(i)+1 deg(j)=deg(j)+1 nxtou(k)=fou(i) fou(i)=k nxtin(k)=fin(j) fin(j)=k ELSE y4(k)=1 ENDIF ELSE nxtou(k)=fou(i) fou(i)=k ENDIF 1915 CONTINUE C Initialize y2k DO 515 k=1,nd IF (y4(k).EQ.0) THEN i=head(k) j=tail(k) IF ((tr(i).GT.tr_tol(i)).AND.(tr(j).GT.tr_tol(j))) THEN y2(k)=x(1,i)*x(1,j)+x(2,i)*x(2,j) ENDIF ENDIF 515 CONTINUE C initialize y3k DO 505 k=1,ndd IF (y5(k).EQ.0) THEN i=head2(k) j=tail2(k) IF ((tr(i).GT.tr_tol(i)).AND.(tr(j).GT.tr_tol(j))) THEN y3(k)=x(1,i)*x(1,j)+x(2,i)*x(2,j)-2*r(k) ENDIF ENDIF 505 CONTINUE C niter=0 CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Loop for approaching the central path C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC DO WHILE (mu.GE.mu_fin) OBJ=0.D0 DO 517 k=1,nd IF (y4(k).EQ.0) THEN i=head(k) j=tail(k) IF (tr(i).GT.tr_tol(i)) THEN IF (tr(j).GT.tr_tol(j)) THEN dd(k)=y(i)-2D0*y2(k)+y(j)-d(k)**2 tii=y(i)-x(1,i)**2-x(2,i)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=y2(k)-x(1,i)*x(1,j)-x(2,i)*x(2,j) dm=tii*tjj-tij**2 IF (dm.le.meps) THEN PRINT*, '1', dm, k PAUSE ENDIF OBJ=OBJ-mu*DLOG(dm) ELSE dd(k)=y(i)-2D0*x(1,i)*x(1,j)-2D0*x(2,i)*x(2,j)+ $ x(1,j)**2+x(2,j)**2-d(k)**2 ENDIF tmp=(DMAX1(0.D0,dd(k)-rho(k)))**2+ $ (DMAX1(0.D0,-dd(k)-rho(k)))**2 OBJ=OBJ+tmp/2D0 ELSE IF (tr(j).GT.tr_tol(j)) THEN dd(k)=y(j)-2D0*x(1,i)*x(1,j)-2D0*x(2,i)*x(2,j)+ $ x(1,i)**2+x(2,i)**2-d(k)**2 tmp=(DMAX1(0.D0,dd(k)-rho(k)))**2+ $ (DMAX1(0.D0,-dd(k)-rho(k)))**2 OBJ=OBJ+tmp/2D0 ENDIF ENDIF ENDIF 517 CONTINUE C Compute terms corresponding to lb constraints DO 507 k=1,ndd IF (y5(k).EQ.0) THEN i=head2(k) j=tail2(k) IF (tr(i).GT.tr_tol(i)) THEN IF (tr(j).GT.tr_tol(j)) THEN rr(k)=y(i)-2D0*y3(k)+y(j)-r(k) tii=y(i)-x(1,i)**2-x(2,i)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=y3(k)-x(1,i)*x(1,j)-x(2,i)*x(2,j) dm=tii*tjj-tij**2 IF (dm.le.meps) THEN PRINT*, '2' PAUSE ENDIF IF (rr(k).le.meps) THEN PRINT*, '30' PAUSE ENDIF OBJ=OBJ-mu*DLOG(dm) ELSE rr(k)=y(i)-2D0*x(1,i)*x(1,j)-2D0*x(2,i)*x(2,j)+ $ x(1,j)**2+x(2,j)**2-r(k) IF (rr(k).le.meps) THEN PRINT*, '31' PAUSE ENDIF ENDIF OBJ=OBJ-mu*reg*DLOG(rr(k)) ELSE IF (tr(j).GT.tr_tol(j)) THEN rr(k)=y(j)-2D0*x(1,i)*x(1,j)-2D0*x(2,i)*x(2,j)+ $ x(1,i)**2+x(2,i)**2-r(k) IF (rr(k).le.meps) THEN PRINT*, '32', niter, rr(k), r(k), y(j) PAUSE ENDIF OBJ=OBJ-mu*reg*DLOG(rr(k)) ENDIF ENDIF ENDIF 507 CONTINUE DO 508 i=1,m IF (tr(i).GT.tr_tol(i)) THEN OBJ=OBJ-mu*DLOG(y(i)-x(1,i)**2-x(2,i)**2) ENDIF 508 CONTINUE ndone=0 nquit=0 C done(i) = 0 if i needs to be iterated DO 518 i=1,m IF (tr(i).GT.tr_tol(i)) THEN done(i)=0 ELSE done(i)=1 ENDIF 518 CONTINUE C enters the loop for LPCGD with lb constraints DO WHILE (ndone+nquit.LT.m) ndone=0 nquit=0 DO 520 i=1,m IF (done(i).EQ.0) THEN niter=niter+1 CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C block-coordinate gradient descent iteration for sensor i C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC gxi(1)=0.D0 gxi(2)=0.D0 gyi=0.D0 obji=0.D0 normg2=0.D0 nnb=0 do 525 k=1,6 525 H(k)=0.d0 xi(1)=x(1,i) xi(2)=x(2,i) tii=y(i)-xi(1)**2-xi(2)**2 tii2=tii**2 k=fou(i) DO WHILE (k.GT.0) j=tail(k) tmp0=DMAX1(0.D0,dd(k)-rho(k)) tmp1=DMAX1(0.D0,-dd(k)-rho(k)) tmp=tmp0-tmp1 obji=obji+tmp0**2+tmp1**2 gyi=gyi+tmp IF (tr(j).LE.tr_tol(j)) THEN gxi(1)=gxi(1)-2D0*tmp*x(1,j) gxi(2)=gxi(2)-2D0*tmp*x(2,j) if (tmp0+tmp1.gt.0) then c Add quadratic-penalty Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+4D0*x(1,j)**2 H(2)=H(2)+4D0*x(1,j)*x(2,j) H(3)=H(3)-2D0*x(1,j) H(4)=H(4)+4D0*x(2,j)**2 H(5)=H(5)-2D0*x(2,j) H(6)=H(6)+1D0 end if ELSE C Construct the entries of Y-X'*X corresponding to i and j tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=y2(k)-xi(1)*x(1,j)-xi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 1a: ',i,k,j,dm pause end if obji=obji-2D0*mu*DLOG(dm) tx1=tij*x(1,j)-tjj*xi(1) tx2=tij*x(2,j)-tjj*xi(2) gxi(1)=gxi(1)-2D0*mu*tx1/dm gxi(2)=gxi(2)-2D0*mu*tx2/dm gyi=gyi-mu*tjj/dm nnb=nnb+1 gy2(nnb)=(-2D0)*tmp+2D0*mu*tij/dm normg2=normg2+gy2(nnb)**2 c Log-barrier Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+mu*(2D0*(tjj+x(1,j)**2)+4D0*tx1**2/dm)/dm H(2)=H(2)+mu*(2D0*x(1,j)*x(2,j)+4D0*tx1*tx2/dm)/dm H(3)=H(3)+mu*2D0*tjj*tx1/dm**2 H(4)=H(4)+mu*(2D0*(tjj+x(2,j)**2)+4D0*tx2**2/dm)/dm H(5)=H(5)+mu*2D0*tjj*tx2/dm**2 H(6)=H(6)+mu*(tjj/dm)**2 C Log-barrier Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=mu*(2D0+4D0*tij**2/dm)/dm Hxy(1,nnb)=mu*((-2D0)*x(1,j)-4D0*tij*tx1/dm)/dm Hxy(2,nnb)=mu*((-2D0)*x(2,j)-4D0*tij*tx2/dm)/dm Hxy(3,nnb)=(-mu)*2D0*tij*tjj/dm**2 if (tmp0+tmp1.gt.0.D0) then C Add quadratic-penalty Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=Hy(nnb)+4D0 Hxy(3,nnb)=Hxy(3,nnb)-2D0 H(6)=H(6)+1D0 end if END IF k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) tmp0=DMAX1(0.D0,dd(k)-rho(k)) tmp1=DMAX1(0.D0,-dd(k)-rho(k)) tmp=tmp0-tmp1 obji=obji+tmp0**2+tmp1**2 gyi=gyi+tmp IF (tr(j).LE.tr_tol(j)) THEN gxi(1)=gxi(1)-2D0*tmp*x(1,j) gxi(2)=gxi(2)-2D0*tmp*x(2,j) if (tmp0+tmp1.gt.0) then c Add quadratic-penalty Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+4D0*x(1,j)**2 H(2)=H(2)+4D0*x(1,j)*x(2,j) H(3)=H(3)-2D0*x(1,j) H(4)=H(4)+4D0*x(2,j)**2 H(5)=H(5)-2D0*x(2,j) H(6)=H(6)+1D0 end if ELSE C Construct the entries of Y-X'*X corresponding to i and j tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=y2(k)-xi(1)*x(1,j)-xi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 1a: ',i,k,j,dm pause end if obji=obji-2D0*mu*DLOG(dm) tx1=tij*x(1,j)-tjj*xi(1) tx2=tij*x(2,j)-tjj*xi(2) gxi(1)=gxi(1)-2D0*mu*tx1/dm gxi(2)=gxi(2)-2D0*mu*tx2/dm gyi=gyi-mu*tjj/dm nnb=nnb+1 gy2(nnb)=(-2D0)*tmp+2D0*mu*tij/dm normg2=normg2+gy2(nnb)**2 c Log-barrier Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+mu*(2D0*(tjj+x(1,j)**2)+4D0*tx1**2/dm)/dm H(2)=H(2)+mu*(2D0*x(1,j)*x(2,j)+4D0*tx1*tx2/dm)/dm H(3)=H(3)+mu*2D0*tjj*tx1/dm**2 H(4)=H(4)+mu*(2D0*(tjj+x(2,j)**2)+4D0*tx2**2/dm)/dm H(5)=H(5)+mu*2D0*tjj*tx2/dm**2 H(6)=H(6)+mu*(tjj/dm)**2 C Log-barrier Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=mu*(2D0+4D0*tij**2/dm)/dm Hxy(1,nnb)=mu*((-2D0)*x(1,j)-4D0*tij*tx1/dm)/dm Hxy(2,nnb)=mu*((-2D0)*x(2,j)-4D0*tij*tx2/dm)/dm Hxy(3,nnb)=(-mu)*2D0*tij*tjj/dm**2 if (tmp0+tmp1.gt.0.D0) then C Add quadratic-penalty Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=Hy(nnb)+4D0 Hxy(3,nnb)=Hxy(3,nnb)-2D0 H(6)=H(6)+1D0 end if END IF k=nxtin(k) END DO C Add the lb constraints terms k=fou2(i) DO WHILE (k.GT.0) j=tail2(k) IF (tr(j).LE.tr_tol(j)) THEN obji=obji-2D0*mu*reg*DLOG(rr(k)) gyi=gyi-mu*reg/rr(k) gxi(1)=gxi(1)+2D0*mu*reg*x(1,j)/rr(k) gxi(2)=gxi(2)+2D0*mu*reg*x(2,j)/rr(k) rr2=(rr(k))**2 c Add Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+4D0*mu*reg*(x(1,j)/rr(k))**2 H(2)=H(2)+4D0*mu*reg*x(1,j)*x(2,j)/rr2 H(3)=H(3)-2D0*mu*reg*x(1,j)/rr2 H(4)=H(4)+4D0*mu*reg*(x(2,j)/rr(k))**2 H(5)=H(5)-2D0*mu*reg*x(2,j)/rr2 H(6)=H(6)+mu*reg/rr2 ELSE C Construct the entries of Y-X'*X corresponding to i and j tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=y3(k)-xi(1)*x(1,j)-xi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 1a: ',i,k,j,dm pause end if obji=obji-2D0*mu*reg*DLOG(rr(k))-2D0*mu*DLOG(dm) tx1=tij*x(1,j)-tjj*xi(1) tx2=tij*x(2,j)-tjj*xi(2) gxi(1)=gxi(1)-2D0*mu*tx1/dm gxi(2)=gxi(2)-2D0*mu*tx2/dm gyi=gyi-mu*tjj/dm-mu*reg/rr(k) nnb=nnb+1 gy2(nnb)=2D0*mu*tij/dm+2D0*mu*reg/rr(k) normg2=normg2+gy2(nnb)**2 rr2=(rr(k))**2 c Log-barrier Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+mu*(2D0*(tjj+x(1,j)**2)+4D0*tx1**2/dm)/dm H(2)=H(2)+mu*(2D0*x(1,j)*x(2,j)+4D0*tx1*tx2/dm)/dm H(3)=H(3)+mu*2D0*tjj*tx1/dm**2 H(4)=H(4)+mu*(2D0*(tjj+x(2,j)**2)+4D0*tx2**2/dm)/dm H(5)=H(5)+mu*2D0*tjj*tx2/dm**2 H(6)=H(6)+mu*(tjj/dm)**2+mu*reg/rr2 C Log-barrier Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=mu*(2D0+4D0*tij**2/dm)/dm+4*mu*reg/rr2 Hxy(1,nnb)=mu*((-2D0)*x(1,j)-4D0*tij*tx1/dm)/dm Hxy(2,nnb)=mu*((-2D0)*x(2,j)-4D0*tij*tx2/dm)/dm Hxy(3,nnb)=(-mu)*2D0*tij*tjj/dm**2-2*mu*reg/rr2 END IF k=nxtou2(k) END DO k=fin2(i) DO WHILE (k.GT.0) j=head2(k) IF (tr(j).LE.tr_tol(j)) THEN obji=obji-2D0*mu*reg*DLOG(rr(k)) gyi=gyi-mu*reg/rr(k) gxi(1)=gxi(1)+2D0*mu*reg*x(1,j)/rr(k) gxi(2)=gxi(2)+2D0*mu*reg*x(2,j)/rr(k) rr2=(rr(k))**2 c Add Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+4D0*mu*reg*(x(1,j)/rr(k))**2 H(2)=H(2)+4D0*mu*reg*x(1,j)*x(2,j)/rr2 H(3)=H(3)-2D0*mu*reg*x(1,j)/rr2 H(4)=H(4)+4D0*mu*reg*(x(2,j)/rr(k))**2 H(5)=H(5)-2D0*mu*reg*x(2,j)/rr2 H(6)=H(6)+mu*reg/rr2 ELSE C Construct the entries of Y-X'*X corresponding to i and j tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=y3(k)-xi(1)*x(1,j)-xi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 1a: ',i,k,j,dm pause end if obji=obji-2D0*mu*reg*DLOG(rr(k))-2D0*mu*DLOG(dm) tx1=tij*x(1,j)-tjj*xi(1) tx2=tij*x(2,j)-tjj*xi(2) gxi(1)=gxi(1)-2D0*mu*tx1/dm gxi(2)=gxi(2)-2D0*mu*tx2/dm gyi=gyi-mu*tjj/dm-mu*reg/rr(k) nnb=nnb+1 gy2(nnb)=2D0*mu*tij/dm+2D0*mu*reg/rr(k) normg2=normg2+gy2(nnb)**2 rr2=(rr(k))**2 c Log-barrier Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+mu*(2D0*(tjj+x(1,j)**2)+4D0*tx1**2/dm)/dm H(2)=H(2)+mu*(2D0*x(1,j)*x(2,j)+4D0*tx1*tx2/dm)/dm H(3)=H(3)+mu*2D0*tjj*tx1/dm**2 H(4)=H(4)+mu*(2D0*(tjj+x(2,j)**2)+4D0*tx2**2/dm)/dm H(5)=H(5)+mu*2D0*tjj*tx2/dm**2 H(6)=H(6)+mu*(tjj/dm)**2+mu*reg/rr2 C Log-barrier Hessian w.r.t. y_{ij} and y_{ii} Hy(nnb)=mu*(2D0+4D0*tij**2/dm)/dm+4*mu*reg/rr2 Hxy(1,nnb)=mu*((-2D0)*x(1,j)-4D0*tij*tx1/dm)/dm Hxy(2,nnb)=mu*((-2D0)*x(2,j)-4D0*tij*tx2/dm)/dm Hxy(3,nnb)=(-mu)*2D0*tij*tjj/dm**2-2*mu*reg/rr2 END IF k=nxtin2(k) END DO c Add 2nd log-barrier Hessian w.r.t. x_i and y_{ii} H(1)=H(1)+mu*(2D0+4D0*xi(1)**2/tii)/tii H(2)=H(2)+mu*4D0*xi(1)*xi(2)/tii2 H(3)=H(3)-mu*2D0*xi(1)/tii2 H(4)=H(4)+mu*(2D0+4D0*xi(2)**2/tii)/tii H(5)=H(5)-mu*2D0*xi(2)/tii2 H(6)=H(6)+mu/tii2 gyi=gyi-mu/tii gxi(1)=gxi(1)+2D0*mu*xi(1)/tii gxi(2)=gxi(2)+2D0*mu*xi(2)/tii normg2=DSQRT(normg2+gxi(1)**2+gxi(2)**2+gyi**2) obji=obji/2D0 obji=obji-mu*DLOG(tii) CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Check stopping criterion C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC IF (normg2.LT.toll) THEN done(i)=1 ndone=ndone+1 ELSE CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Cholesky factorization of Hessian (arrow sparsity) H=L*L' C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC do 530 k=1,6 530 LS(k)=0.d0 do 531 k=1,nnb Ly(k)=dsqrt(Hy(k)) Lxy(1,k)=Hxy(1,k)/Ly(k) Lxy(2,k)=Hxy(2,k)/Ly(k) Lxy(3,k)=Hxy(3,k)/Ly(k) LS(1)=LS(1)+Lxy(1,k)**2 LS(2)=LS(2)+Lxy(1,k)*Lxy(2,k) LS(3)=LS(3)+Lxy(1,k)*Lxy(3,k) LS(4)=LS(4)+Lxy(2,k)**2 LS(5)=LS(5)+Lxy(2,k)*Lxy(3,k) LS(6)=LS(6)+Lxy(3,k)**2 531 continue if (H(1).le.LS(1)) then print*,i,1,H(1)-LS(1) pause nquit=nquit+1 GO TO 520 end if L(1)=dsqrt(H(1)-LS(1)) L(2)=(H(2)-LS(2))/L(1) L(3)=(H(3)-LS(3))/L(1) if (H(4).le.LS(4)+L(2)**2) then print*,i,4,H(4)-LS(4)-L(2)**2 pause nquit=nquit+1 GO TO 520 end if L(4)=dsqrt(H(4)-LS(4)-L(2)**2) L(5)=(H(5)-LS(5)-L(2)*L(3))/L(4) if (H(6).le.LS(6)+L(3)**2+L(5)**2) then print*,i,6,H(6)-LS(6)-L(3)**2-L(5)**2 print*,H(6), LS(6) pause nquit=nquit+1 GO TO 520 end if L(6)=dsqrt(H(6)-LS(6)-L(3)**2-L(5)**2) C Solve L*w=-g for w w1=-gxi(1) w2=-gxi(2) w3=-gyi do 535 k=1,nnb w(k)=(-gy2(k))/Ly(k) w1=w1-Lxy(1,k)*w(k) w2=w2-Lxy(2,k)*w(k) w3=w3-Lxy(3,k)*w(k) 535 continue w1=w1/L(1) w2=(w2-L(2)*w1)/L(4) w3=(w3-L(3)*w1-L(5)*w2)/L(6) C Solve L'*d=w for descent direction dir & compute dir'*g d3=w3/L(6) d2=(w2-L(5)*d3)/L(4) d1=(w1-L(2)*d2-L(3)*d3)/L(1) dderiv=d1*gxi(1)+d2*gxi(2)+d3*gyi do 540 k=nnb,1,-1 dir(k)=(w(k)-Lxy(1,k)*d1-Lxy(2,k)*d2- $ Lxy(3,k)*d3)/Ly(k) dderiv=dderiv+gy2(k)*dir(k) 540 continue CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Initialize the stepsize in Armijo line search along dir C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC oldobj=obj C Initialize stepsize so that it's below 1 & y_ii^new > ||x_i^new||^2 a=d1**2+d2**2 c=d3-2D0*(x(1,i)*d1+x(2,i)*d2) if (c.ge.0.D0) then step=(c+dsqrt(c**2+4d0*a*tii))/(2D0*a)*0.99D0 else step=2D0*tii/(dsqrt(c**2+4d0*a*tii)-c)*0.99D0 endif if (step.gt.1.D0) then step=1.D0 end if newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 IF (1.eq.0) THEN step=1.D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 DO WHILE (tii.LE.0.D0) step=step/2.D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 END DO ENDIF c print*,'dderiv,step,tr_i(Ztrial),a,c = ',dderiv,step,tii,a,c C Decrease step if not feasible for all nbhr j sequentially. (This C does NOT ensure the stepsize is feasible, but it's only one pass and C yields a feasible stepsize most of the times.) nnb=0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF (tr(j).GT.tr_tol(j)) THEN nnb=nnb+1 newyij=y2(k)+step*dir(nnb) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 end if END IF k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) IF (tr(j).GT.tr_tol(j)) THEN nnb=nnb+1 newyij=y2(k)+step*dir(nnb) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 end if END IF k=nxtin(k) END DO k=fou2(i) DO WHILE (k.GT.0) j=tail2(k) IF (tr(j).LE.tr_tol(j)) THEN rrr=newyi-2D0*x(1,j)*newxi(1)-2D0*x(2,j)*newxi(2) $ +x(1,j)**2+x(2,j)**2-r(k) if (rrr.le.meps) then step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 endif ELSE nnb=nnb+1 newyij=y3(k)+step*dir(nnb) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 rrr=newyi-2*newyij+y(j)-r(k) if ((dm.le.meps).OR.(rrr.le.meps)) then step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 end if END IF k=nxtou2(k) END DO k=fin2(i) DO WHILE (k.GT.0) j=head2(k) IF (tr(j).LE.tr_tol(j)) THEN rrr=newyi-2D0*x(1,j)*newxi(1)-2D0*x(2,j)*newxi(2) $ +x(1,j)**2+x(2,j)**2-r(k) if (rrr.le.meps) then step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 endif ELSE nnb=nnb+1 newyij=y3(k)+step*dir(nnb) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 rrr=newyi-2*newyij+y(j)-r(k) if ((dm.le.meps).OR.(rrr.le.meps)) then step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 end if END IF k=nxtin2(k) END DO C Evaluate the part of the new objective value involving y_i, x_i, y_{ij} C & decrease step further if not feasible for all nbhr j simultaneously. C This ensures the stepsize is feasible, though multiple passes may be C needed occasionally. 550 nobji=0.d0 nnb=0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF (tr(j).LE.tr_tol(j)) THEN ddd=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)*x(1,j)+x(2,j)*x(2,j)-d(k)**2 ELSE nnb=nnb+1 newyij=y2(k)+step*dir(nnb) ddd=newyi-2D0*newyij+y(j)-d(k)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then step=0.75D0*step newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 go to 550 end if C IF ((niter.GT.4051150).and.(k.eq.2500)) THEN C PRINT*, i,k,dm C ENDIF nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji+(DMAX1(0.D0,ddd-rho(k)))**2+ $ (DMAX1(0.D0,-ddd-rho(k)))**2 k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) IF (tr(j).LE.tr_tol(j)) THEN ddd=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)*x(1,j)+x(2,j)*x(2,j)-d(k)**2 ELSE nnb=nnb+1 newyij=y2(k)+step*dir(nnb) ddd=newyi-2D0*newyij+y(j)-d(k)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then step=0.75D0*step newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 go to 550 end if C IF ((niter.GT.4051150).and.(k.eq.2500)) THEN C PRINT*, i,k,dm C ENDIF nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji+(DMAX1(0.D0,ddd-rho(k)))**2+ $ (DMAX1(0.D0,-ddd-rho(k)))**2 k=nxtin(k) END DO k=fou2(i) DO WHILE (k.GT.0) j=tail2(k) IF (tr(j).LE.tr_tol(j)) THEN rrr=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)**2+x(2,j)**2-r(k) if (rrr.le.meps) then step=0.75D0*step newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 go to 550 endif ELSE nnb=nnb+1 newyij=y3(k)+step*dir(nnb) rrr=newyi-2D0*newyij+y(j)-r(k) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if ((dm.le.meps).OR.(rrr.le.meps)) then step=0.75D0*step newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 go to 550 end if nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji-2D0*mu*reg*DLOG(rrr) k=nxtou2(k) END DO k=fin2(i) DO WHILE (k.GT.0) j=head2(k) IF (tr(j).LE.tr_tol(j)) THEN rrr=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)**2+x(2,j)**2-r(k) if (rrr.le.meps) then step=0.75D0*step newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 go to 550 endif ELSE nnb=nnb+1 newyij=y3(k)+step*dir(nnb) rrr=newyi-2D0*newyij+y(j)-r(k) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if ((dm.le.meps).OR.(rrr.le.meps)) then step=0.75D0*step newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 go to 550 end if nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji-2D0*mu*reg*DLOG(rrr) k=nxtin2(k) END DO nobji=nobji/2D0 nobji=nobji-mu*DLOG(tii) c print*,' feas. step= ',step,' new obj= ',nobji,' old obj= ',obji CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Armijo rule C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC DO WHILE (((nobji-obji).GT.(0.1D0*step*dderiv)) $ .AND.(step.GT.1d-5)) 560 step=step/2D0 newxi(1)=x(1,i)+step*d1 newxi(2)=x(2,i)+step*d2 newyi=y(i)+step*d3 tii=newyi-newxi(1)**2-newxi(2)**2 C Evaluate the part of the new objective value involving y_{ii}, x_i, y_{ij} nobji=0.d0 nnb=0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF (tr(j).LE.tr_tol(j)) THEN ddd=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)**2+x(2,j)**2-d(k)**2 ELSE nnb=nnb+1 newyij=y2(k)+step*dir(nnb) ddd=newyi-2D0*newyij+y(j)-d(k)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 3a: ',i,k,j,dm pause go to 560 end if nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji+(DMAX1(0.D0,ddd-rho(k)))**2+ $ (DMAX1(0.D0,-ddd-rho(k)))**2 k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) IF (tr(j).LE.tr_tol(j)) THEN ddd=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)**2+x(2,j)**2-d(k)**2 ELSE nnb=nnb+1 newyij=y2(k)+step*dir(nnb) ddd=newyi-2D0*newyij+y(j)-d(k)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 3a: ',i,k,j,dm pause go to 560 end if nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji+(DMAX1(0.D0,ddd-rho(k)))**2+ $ (DMAX1(0.D0,-ddd-rho(k)))**2 k=nxtin(k) END DO k=fou2(i) DO WHILE (k.GT.0) j=tail2(k) IF (tr(j).LE.tr_tol(j)) THEN rrr=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)**2+x(2,j)**2-r(k) ELSE nnb=nnb+1 newyij=y3(k)+step*dir(nnb) rrr=newyi-2D0*newyij+y(j)-r(k) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 3a: ',i,k,j,dm pause go to 560 end if nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji-2d0*mu*reg*DLOG(rrr) k=nxtou2(k) END DO k=fin2(i) DO WHILE (k.GT.0) j=head2(k) IF (tr(j).LE.tr_tol(j)) THEN rrr=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2)*x(2,j) $ +x(1,j)**2+x(2,j)**2-r(k) ELSE nnb=nnb+1 newyij=y3(k)+step*dir(nnb) rrr=newyi-2D0*newyij+y(j)-r(k) tjj=y(j)-x(1,j)**2-x(2,j)**2 tij=newyij-newxi(1)*x(1,j)-newxi(2)*x(2,j) dm=tii*tjj-tij**2 if (dm.le.meps) then print*,'dm<=0! 3a: ',i,k,j,dm pause go to 560 end if nobji=nobji-2D0*mu*DLOG(dm) END IF nobji=nobji-2d0*mu*reg*DLOG(rrr) k=nxtin2(k) END DO nobji=nobji/2D0 nobji=nobji-mu*DLOG(tii) END DO IF (step.LT.1d-5) THEN C Can choose a bit larger than 1d-5 nquit=nquit+1 GO TO 520 ELSE C IF (step.LT.2d-3) THEN C PRINT*, step, i, tr(i), tr_tol(i) C PAUSE C ENDIF IF (nobji.GT.obji) THEN PRINT*,'objective value increases' PRINT*,'Press to continue' pause nquit=nquit+1 GO TO 520 END IF C Accept the current stepsize and the new y_{ii}, x_i, y_{ij} obj=oldobj+nobji-obji x(1,i)=newxi(1) x(2,i)=newxi(2) y(i)=newyi nnb=0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF (tr(j).LE.tr_tol(j)) THEN dd(k)=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2) $ *x(2,j)+x(1,j)**2+x(2,j)**2-d(k)**2 ELSE nnb=nnb+1 y2(k)=y2(k)+step*dir(nnb) dd(k)=newyi-2D0*y2(k)+y(j)-d(k)**2 done(j)=0 END IF k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) IF (tr(j).LE.tr_tol(j)) THEN dd(k)=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2) $ *x(2,j)+x(1,j)**2+x(2,j)**2-d(k)**2 ELSE nnb=nnb+1 y2(k)=y2(k)+step*dir(nnb) dd(k)=newyi-2D0*y2(k)+y(j)-d(k)**2 done(j)=0 END IF k=nxtin(k) END DO k=fou2(i) DO WHILE (k.GT.0) j=tail2(k) IF (tr(j).LE.tr_tol(j)) THEN rr(k)=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2) $ *x(2,j)+x(1,j)**2+x(2,j)**2-r(k) ELSE nnb=nnb+1 y3(k)=y3(k)+step*dir(nnb) rr(k)=newyi-2D0*y3(k)+y(j)-r(k) done(j)=0 END IF k=nxtou2(k) END DO k=fin2(i) DO WHILE (k.GT.0) j=head2(k) IF (tr(j).LE.tr_tol(j)) THEN rr(k)=newyi-2D0*newxi(1)*x(1,j)-2D0*newxi(2) $ *x(2,j)+x(1,j)**2+x(2,j)**2-r(k) ELSE nnb=nnb+1 y3(k)=y3(k)+step*dir(nnb) rr(k)=newyi-2D0*y3(k)+y(j)-r(k) done(j)=0 END IF k=nxtin2(k) END DO if (nobji-obji.ge.(-mu_fin)*(dabs(obji)+1D0)) then c print*,'tiny change in obj! ' c print*,(nobji-obji)/(dabs(obji)+1D0) c pause nquit=nquit+1 end if END IF END IF ELSE ndone=ndone+1 END IF C IF (niter.GT.25500) THEN C print*, ' mu,iter,step,ndone,obj=', C $ mu,niter,step,ndone,obj, tii IF (.NOT.(obj.eq.obj)) THEN pause ENDIF C ENDIF C pause 520 CONTINUE C PRINT*,' mu, iter, #not done, obj, step, nquit', C $ mu, niter, m-ndone, obj, step, nquit C pause END DO mu=mu/factor IF (toll.GT.tol) THEN toll=toll/factor ENDIF END DO CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Save solution to file C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 179 TT = etime(tarr1) TT = tarr1(1)-TIMER truOBJ=0.D0 DO 110 k=1,nd j=tail(k) IF (j.LE.m) THEN IF (y4(k).eq.1) THEN i=head(k) C Choose yij to make the cost zero. Does this satisfy the psd constraint? tii=y(i)-x(1,i)**2-x(2,i)**2 tjj=y(j)-x(1,j)**2-x(2,j)**2 tmp0=(y(i)+y(j)-d(k)**2)/2D0 tmp1=x(1,i)*x(1,j)+x(2,i)*x(2,j) tij=tmp0-tmp1 dm=tii*tjj-tij**2 IF (dm.lt.0) THEN C set y(i,j) to minimize the ESDP cost while satisfying the psd constraints y2(k)=dsign(dsqrt(tii*tjj),tmp1-tmp0)+tmp1 dd(k)=y(i)-2D0*y2(k)+y(j)-d(k)**2 ELSE y2(k)=tmp0 dd(k)=0d0 ENDIF ENDIF ENDIF tmp=(DMAX1(0.D0,dd(k)-rho(k)))**2+ $ (DMAX1(0.D0,-dd(k)-rho(k)))**2 truOBJ=truOBJ+tmp 110 CONTINUE truOBJ=truOBJ/2D0 PRINT*,'TOTAL TIME = ',TT, ' secs.' PRINT*,' rho-ESDP obj= ',OBJ,' true rho-ESDP obj (all edges)= ', $truOBJ PRINT*,' iter= ',niter,' nquit= ',nquit C IF (MODEL.EQ.1) THEN C PRINT*,' Noisy factor= ', sigma C ELSE C PRINT*,' Noisy factor= ', (5*sigma)**2 C ENDIF DO 120 i=1,m sol(1,i)=x(1,i) sol(2,i)=x(2,i) tr(i)=y(i)-x(1,i)**2-x(2,i)**2 120 CONTINUE PRINT*, ' Enter 1 for another round of lb refinement, else 2' READ*, Refine IF (Refine.EQ.1) THEN GO TO 1779 ENDIF OPEN(12,FILE='sensorsoln3.dat',STATUS='NEW') REWIND(12) DO 130 i=1,m WRITE(12,1021) sol(1,I), sol(2,I), tr(I) 130 CONTINUE CLOSE(12) PRINT*,'Done writing file' PRINT*,'Enter 1 for gradient descent, enter 2 to quit' READ*,Refine IF (Refine.EQ.1) THEN PRINT*,'Enter 1 to refine all sensors (else refine only sensors $ with large trace) ' READ*, trtol C PRINT*, 'Enter gradient tolerance (1d-3)' C READ*, eps eps=1d-3 T3 = etime(tarr3) T3 = tarr3(1) C Initialize trace tolerance IF (trtol.EQ.1) THEN DO 180 I=1,m 180 tr_tol(i)=0.D0 ELSE tmp=1d-2+30D0*sigma DO 190 I=1,m s=0 dis=0.D0 k=fou(i) DO WHILE (k.GT.0) s=s+1 dis=dis+d(k) k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) s=s+1 dis=dis+d(k) k=nxtin(k) END DO IF (s.GT.0) THEN dis=(dis/s)**2 ELSE dis=0.D0 ENDIF tr_tol(i)=dis*tmp 190 CONTINUE ENDIF C Add back all those deleted edges IF (degbd.LT.100) THEN PRINT*, ' enter 1 to add back edges, 2 to skip' READ*, Addbk2 ENDIF IF (Addbk2.eq.1) THEN DO 200 I=1,m FIN(I)=0 FOU(I)=0 200 CONTINUE DO 210 k=1,nd i=head(k) j=tail(k) IF (j.LE.m) THEN nxtou(k)=fou(i) fou(i)=k nxtin(k)=fin(j) fin(j)=k ELSE nxtou(k)=fou(i) fou(i)=k ENDIF 210 CONTINUE ENDIF ndone=0 nquit=0 niter=0 C Compute initial function value obj2=0.D0 DO 220 k=1,nd i=head(k) j=tail(k) tij=DSQRT((x(1,i)-x(1,j))**2+(x(2,i)-x(2,j))**2) obj2=obj2+(tij-d(k))**2 220 CONTINUE m2=0 DO 230 I=1,m IF (y(i).gt.tr_tol(i)) THEN done(i)=0 m2=m2+1 ELSE done(i)=1 ENDIF 230 CONTINUE DO WHILE ((nquit+ndone).LT.m) ndone=0 nquit=0 DO 300 I=1,m IF (done(i).EQ.0) THEN niter=niter+1 obj2i=0.D0 gxi(1)=0.D0 gxi(2)=0.D0 C Compute the function value and gradient for the i th part k=fou(i) DO WHILE (k.GT.0) j=tail(k) tij=DSQRT((x(1,i)-x(1,j))**2+(x(2,i)-x(2,j))**2) IF (tij.LE.meps) THEN nquit=nquit+1 GO TO 300 ENDIF obj2i=obj2i+(tij-d(k))**2 gxi(1)=gxi(1)+2D0*(1D0-d(k)/tij)*(x(1,i)-x(1,j)) gxi(2)=gxi(2)+2D0*(1D0-d(k)/tij)*(x(2,i)-x(2,j)) k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) tij=DSQRT((x(1,i)-x(1,j))**2+(x(2,i)-x(2,j))**2) IF (tij.LE.meps) THEN nquit=nquit+1 GO TO 300 ENDIF obj2i=obj2i+(tij-d(k))**2 gxi(1)=gxi(1)+2D0*(1D0-d(k)/tij)*(x(1,i)-x(1,j)) gxi(2)=gxi(2)+2D0*(1D0-d(k)/tij)*(x(2,i)-x(2,j)) k=nxtin(k) END DO normg2=gxi(1)**2+gxi(2)**2 IF (normg2.LE.eps**2) THEN done(i)=1 ndone=ndone+1 ELSE step=1 newxi(1)=x(1,i)-step*gxi(1) newxi(2)=x(2,i)-step*gxi(2) C Compute new function value nobj2i=0.D0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) tij=DSQRT((newxi(1)-x(1,j))**2+(newxi(2)-x(2,j))**2) nobj2i=nobj2i+(tij-d(k))**2 k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) tij=DSQRT((newxi(1)-x(1,j))**2+(newxi(2)-x(2,j))**2) nobj2i=nobj2i+(tij-d(k))**2 k=nxtin(k) END DO C Armijo Rule DO WHILE (nobj2i.GT.(obj2i-0.1D0*step*normg2)) IF (STEP.LT.1d-5) THEN nquit=nquit+1 GO TO 300 ENDIF step=step/2D0 newxi(1)=x(1,i)-step*gxi(1) newxi(2)=x(2,i)-step*gxi(2) C Compute new function value nobj2i=0.D0 k=fou(i) DO WHILE (k.GT.0) j=tail(k) tij=DSQRT((newxi(1)-x(1,j))**2+(newxi(2)-x(2,j))**2) nobj2i=nobj2i+(tij-d(k))**2 k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) tij=DSQRT((newxi(1)-x(1,j))**2+(newxi(2)-x(2,j))**2) nobj2i=nobj2i+(tij-d(k))**2 k=nxtin(k) END DO END DO IF (nobj2i-obj2i.ge.(-1d-11)*(dabs(obj2i)+1D0)) THEN nquit=nquit+1 GO TO 300 ENDIF x(1,i)=x(1,i)-step*gxi(1) x(2,i)=x(2,i)-step*gxi(2) obj2=obj2-obj2i+nobj2i C Allow neighboring sensors to vary k=fou(i) DO WHILE (k.GT.0) j=tail(k) IF ((j.LE.m).and.(y(j).gt.tr_tol(j))) THEN done(j)=0 ENDIF k=nxtou(k) END DO k=fin(i) DO WHILE (k.GT.0) j=head(k) IF (y(j).gt.tr_tol(j)) THEN done(j)=0 ENDIF k=nxtin(k) END DO END IF ELSE ndone=ndone+1 ENDIF 300 CONTINUE C PRINT*,'iter,obj2,#not done,nquit= ',niter,obj2,m-ndone,nquit END DO TT3 = etime(tarr3) TT3 = tarr3(1)-T3 OPEN(12,FILE='sensorsoln2.dat',STATUS='NEW') REWIND(12) DO 310 i=1,m WRITE(12,1020) x(1,I), x(2,I) 310 CONTINUE CLOSE(12) PRINT*,'end of grad. desc. improvement, time = ',TT3,' sec.' PRINT*,'#sensors treated as anchors = ',m-m2 PRINT*,' Total time = ',TT3+TT,' sec.' END IF STOP END