program dixyz c programmed by c.c.tscherning, Oct. 1, 1993. c Updated 2009-05-29 by CCT. c the program computes distances between one point and c a list of points with given xyz coordinates and observed distances. C The observations are then used in a least-squares adjustment C to determine improvements to the coordinates of the first point. implicit real*8 (a-h,o-z),logical (l) dimension x(100),y(100),no(100),h(100),dd(100),ddn(100), *dx(100,4),hi(10),an(100),s(100) ,ih(100),ant(100) character *72 ifile,ofile logical lt,lf lt=.true. lf=.false. C write(*,*) *' Program to compute point position, 2009-05-28' write(*,*)' input name of file with input data' read(*,'(A)')ifile write(*,'(a)')' Input file name = ',ifile open(12,file=ifile,status='old') lstop=.false. write(*,*)' input name of output file' read(*,'(A)')ofile write(*,'(a)')' Output file name = ',ofile open(14,file=ofile,status='unknown') write(*,*) n=0 10 n=n+1 if (n.eq.1) then read(12,*,end=99)x(n),y(n),h(n) no(1)=100 else read(12,*,end=99)no(n),s(n),x(n),y(n),h(n) read(12,*,end=99)ddn(n) end if go to 10 99 n1=n-1 write(*,*)' number of data',n1 write(*,*) *' Stat. 1 Stat. 2 Dist. (m) dx,dy,dz, obs-comp. ' i=1 j1=i+1 do 100,j=j1,n1 c preliminary coordinates minus satellite coordinates. dx(j,1)=x(1)-x(j) dx(j,2)=y(1)-y(j) dh=h(1)-h(j) dx(j,3)=dh dh=h(j)-h(i) dist=sqrt(dx(j,1)**2+dx(j,2)**2+dh**2) do k=1,3 dx(j,k)=dx(j,k)/dist end do dx(j,4)=1.0d0 write(*,209)no(i),no(j),dist,dx(j,1),dx(j,2),dx(j,3),ddn(j)-dist 209 format(2I10,f14.3,4f8.3) c Observed minus computed distance. dd(j)=ddn(j)-dist 100 continue close(12) write(*,*)' Input standard deviation of observation noise' read(*,*)stdv write(*,135)stdv 135 format(' Standard deviation = ',f6.3, ' m',/) c write(*,*)' Do you want time to be solved for (t/f) ' read(*,*)ltime ndim=3 if (ltime) ndim=4 nadim=ndim*(ndim+1)/2 varian=stdv**2 do n=1,nadim an(n)=0.0d0 if (n.le.ndim) hi(n)=0.0d0 end do do n=2,n1 ih(n-1)=1 j=0 do k=1,ndim do i=1,k j=j+1 an(j)=an(j)+dx(n,k)*dx(n,i)*varian end do hi(k)=hi(k)+dx(n,k)*dd(n)*varian end do end do write(*,*)' Upper triangular part of AT(P**-1)A ' if (ndim.eq.3) then write(*,107)(an(k),k=1,nadim) 107 format(f12.3,/,2f12.3,/3f12.3) else write(*,127)(an(k),k=1,nadim) 127 format(f12.3,/,2f12.3,/,3f12.3,/,4f12.3) end if write(*,*)' Right-hand side AT(P**-1)Y ' write(*,108)(hi(i),i=1,ndim) 108 format(4f12.3) call pronll(an,ih,hi,ndim,var,lt,lt,ndim,ndim,ndim) write(*,*)' Solution vector = coordinate improvements ' write(*,108)(hi(i),i=1,3) if (ndim.eq.4) write(*,128)hi(4) 128 format(' c x dt= ',f14.8) cdt=hi(4) x(1)=x(1)+hi(1) y(1)=y(1)+hi(2) h(1)=h(1)+hi(3) write(*,118)x(1),y(1),h(1) 118 format(' New coordinates:',3f14.3,/' Var.-Cov.-matrix'/) write(14,119)x(1),y(1),h(1) 119 format(3f15.3) do k=2,n1 write(14,120)no(k),s(k),x(k),y(k),h(k) 120 format(i6,4f14.3) write(14,119)ddn(k) end do c The inverse matrix is determined by solving for n vectors with c only zeroes, except for a 1.0 at the k'th position. do k=1,ndim do j=1,ndim hi(j)=0.0d0 end do hi(k)=1.0d0 call pronll(an,ih,hi,ndim,var,lf,lt,ndim,ndim,ndim) write(*,117)(hi(i),i=1,ndim) 117 format(4f14.8) end do write(*,*)' Observation no., difference new computed and obs.' do i=2,n1 d1=x(1)-x(i) d2=y(1)-y(i) d3=h(1)-h(i) dist=sqrt(d1**2+d2**2+d3**2) if (ndim.eq.4)dist=dist+cdt resid=dist-ddn(i) write(*,115)i,resid 115 format(i4,10x,f10.4) end do stop end C SUBROUTINE PRONLL(AN,INUL,H,NT,VAR,LRED,LBS,IANT,INULT,IHT) C C THIS SUBROUTINE USES A CHOLESKY ALGORITHME FOR REDUCING C AND SOLVING THE SYSTEM OF LINEAR EQUATIONS C (AT*A)*X=AT*Y C WHERE (AT*A) IS SYMMETRICAL POSITIV DEFINITE MATRIX OF C DIMENSION NT*NT, AND (AT*Y) IS A VECTOR OF DIMENSION NT. C C CONTEND OF ARRAYES: C AN(.) THE UPPER PART OF (AT*A), AND RETURNS C WITH LT, WHERE L*LT=(AT*A), IF LRED = C .TRUE. C INUL(.) INDEX OF THE FIRST NON-ZERO ELEMENT C OF EACH ROW. C H(.) THE RIGHT-HANDSIDE (AT*Y), AND RETURNS C WITH X ,IF LBS = .TRUE., ELSE WITH C (L-1)*(AT*Y). C VAR THE PSEUDO DIAGONAL ELEMENT OF (L-1)* C (AT*Y). C C C PROGRAMMED BY C PER KNUDSEN C GEODETIC INST. C DK-2920 12.07.85. C IMPLICIT REAL*8 (A-H,O-Z) DIMENSION AN(IANT),INUL(INULT),H(IHT) LOGICAL LRED,LBS C C*** THE UPPER PART OF A IS REDUCED INTO LT IF LRED IS TRUE. C IF(.NOT.LRED) GO TO 50 DO 25 IS=1,NT IST=(IS*(IS-1))/2 SUM=0.0D0 DO 10 IR=INUL(IS),(IS-1) IRT=(IR*(IR-1))/2 SUM=0.0D0 DO 5 II=MAX0(INUL(IS),INUL(IR)),(IR-1) SUM=SUM+(AN(IRT+II)*AN(IST+II)) 5 CONTINUE AN(IST+IR)=(AN(IST+IR)-SUM)/AN(IRT+IR) 10 CONTINUE SUM=0.0D0 DO 15 II=INUL(IS),(IS-1) SUM=SUM+AN(IST+II)**2 15 CONTINUE AN(IST+IS)=SQRT(AN(IST+IS)-SUM) 25 CONTINUE C C 50 CONTINUE C*** SOLVE L-1*H C DO 100 IR=1,NT IRT=(IR*(IR-1))/2 SUM=0.0D0 DO 90 II=INUL(IR),(IR-1) SUM=SUM+(AN(IRT+II)*H(II)) 90 CONTINUE H(IR)=(H(IR)-SUM)/AN(IRT+IR) 100 CONTINUE SUM=0.0D0 DO 101 II=1,NT SUM=SUM+H(II)**2 101 CONTINUE VAR=SUM C C*** THE SOLUTION IS FOUND BY BACK SUBSTITUTUION IF LBS IS TRUE. C IF(.NOT.LBS) RETURN C DO 150 IRR=1,NT IR=NT+1-IRR IRT=(IR*(IR-1))/2 SUM=0.0D0 DO 140 II=(IR+1),NT IIT=(II*(II-1))/2 SUM=SUM+(AN(IIT+IR)*H(II)) 140 CONTINUE H(IR)=(H(IR)-SUM)/AN(IRT+IR) 150 CONTINUE C RETURN END