CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Dec 2008 C Update July 29, 2009: A small error in the quadratic C formula for initializing step is corrected. C Initial mu is changed to 0.1 for faster convergence. C Update Aug 4, 2009: A loop that allows users C to choose whether to include the dropped edges in the C refinement phase is added. 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 an optional postprocessing phase that refines the sensor positions 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 d(k) measured distance for the kth edge, k=1,...,nd 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 any 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 Addbk2 1 if deleted edges are to be added back in the refinement stage; C 0 else C mu_init Initial value for mu (default is 1d-1). 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 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 T. K. Pong and P. Tseng, (Robust) Edge-Based Semidefinite Programming Relaxation of C Sensor Network Localization, Report, Dept. Mathematics, Univ. Washington, Seattle, Jan 2009. C C Authors: Ting Kei Pong and Paul Tseng. Questions about the code may be directed to them. C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC PARAMETER (MAXN=10000, MAXA=400000, MAXDEG=103) INTEGER head(MAXA),tail(MAXA),done(MAXN),deg(MAXN) INTEGER FIN(MAXN),FOU(MAXN),NXTIN(MAXA),NXTOU(MAXA) INTEGER ndone,nquit,niter,degbd INTEGER n,na,m,dim,nd,nnb INTEGER Refine,Addbk,Addbk2 REAL*8 sigma,tol,mu_ini,mu_fin,factor REAL*8 mu,gtol,ddd,OBJ,obji REAL*8 sol(2,MAXN),d(MAXA),x(2,MAXN),y(MAXN) REAL*8 y2(MAXA),dd(MAXA),rho(MAXA),gyi REAL*8 gxi(2),gy2(MAXDEG),normg2,tmp0,tmp1,tmp 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 m2,s REAL*8 obj2,obj2i,nobj2i,tr_tol(MAXN),infty,dis,trtol,eps real etime, tarr1(2),TT,T3,TT3,tarr3(2),timer external etime EQUIVALENCE(done,deg) 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 m=n-na PRINT*,' #pts=',n,' #anchors=',na,' #sensors=',m,' #edges=',nd 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 CLOSE(13) 1000 FORMAT(4I8) 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 1 to add back some deleted edges' READ*, Addbk 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=10.d0 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=-1D0+1D0/(1D0-2d0*sigma)**2 DO 11 k=1,nd rho(k)=tmp*(d(k)**2) 11 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 gtol=mu C Build linked list for neighborhood structure in network DO 15 I=1,m FIN(I)=0 FOU(I)=0 C deg(i) will count the #sensor neighbors of sensor i deg(i)=0 15 CONTINUE infty=1d40 DO 16 k=1,nd i=head(k) j=tail(k) 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 y2(k)=infty ENDIF ELSE nxtou(k)=fou(i) fou(i)=k ENDIF 16 CONTINUE CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Loop for approaching the analytic center solution 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.gtol) 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 C pause 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 (y2(k).eq.infty) 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) ENDIF ENDIF ENDIF 70 CONTINUE ENDIF mu=mu/factor IF (gtol.GT.tol) THEN gtol=gtol/factor ENDIF END DO CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C Save solution to file C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC TT = etime(tarr1) TT = tarr1(1)-TIMER truOBJ=0.D0 DO 110 k=1,nd j=tail(k) IF (j.LE.m) THEN IF (y2(k).eq.infty) 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 DO 120 i=1,m sol(1,i)=x(1,i) sol(2,i)=x(2,i) y(i)=y(i)-x(1,i)**2-x(2,i)**2 120 CONTINUE OPEN(12,FILE='sensorsoln.dat',STATUS='NEW') REWIND(12) DO 130 i=1,m WRITE(12,1021) sol(1,I), sol(2,I), y(I) 130 CONTINUE CLOSE(12) PRINT*,'Done writing file' PRINT*,'Enter 1 to do refinement, 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 ' 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