program gpsegi implicit real*8(a-h,o-z) ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c c G P S E G I c c Simple program for merging GPS and INS from 'readegi' (.coo files) c The program fill in GPS blanks with interpolated INS values, c and output composite file (.pos) with pitch, roll and heading c c input: c c egifile (.coo) c gpsfile - must be one sec c tstart, tstop (dechr), tshift (sec, UTC offset), tc, ltrim, ivzint c c ltrim: t for trimble output, f if solution has gpstime in first col c ivzint: 0 = use GPS coor (no .dif file written) c 1 = use EGI with height from integrated velocity c 2 = use EGI with direct heights (note: resolution problem) c tshift: GPS-UTC time offset (+13 sec in 2001) c tc: time constant (sec) for smoothing INS-GPS differences c (tc=0 mean no smoothing; note: if tc>0 the GPS solution is modified) c c Outputfiles: c gpsegi.pos: thr, lat, lon, h (GPS), pitch, roll, az (INS), igap c gpsegi.dif: thr, dlat, dlon, dh (INS-GPS, meter) c c igap: 0 for no gps gap, 1 for interpolation at gaps c c Note: INS-GPS difference has some noise at the m-level (unknown source) c This noise is very sensitive to the correct utc offset c c (c) rf/kms june 2001 c updated cfs alert may 2004 (improved warnings for gps gaps etc) c implemented midnight (avo05) from gpsegikk c warnings for gap and time errors in egi data rf feb 08 c Mode for GPS interpolation instead of EGI, rf oct 08 c Updated jan 2011, rf c ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc character gpsfile*72,egifile*72 logical ltrim,lvzint,lgap,lgps dimension xgps(28801),ygps(28801),hgps(28801), .xdif(28801),ydif(28801),hdif(28801),cosfi(28801),xx(28801), .lgap(28801) maxn = 28801 c degm = 1852*60 radeg = 180/3.14159265d0 posmax = 0 tposmax = 0 hsum2 = 0 nsum2 = 0 c write(*,*) '- GPSEGI -' read(*,'(a)') egifile read(*,'(a)') gpsfile read(*,*) tstart,tstop,tshift,tc,ltrim,ivzint c tshift = tshift/3600 lvzint = (ivzint.eq.1) if (ivzint.gt.0) then lgps = .false. write(*,1) tc 1 format(' draping time constant INS to GPS (sec): ',f6.1) else lgps = .true. write(*,*) 'using GPS instead of EGI for coordinates' if (tc.gt.0) . write(*,*) '*** warning, tc not zero - coor filtered' endif c open(10,file=gpsfile,status='old') open(11,file=egifile,status='old') open(20,file='gpsegi.pos',status='unknown') if (.not.lgps) open(21,file='gpsegi.dif',status='unknown') open(30,status='scratch',form='unformatted') c nn = nint((tstop-tstart)*3600)+1 if (nn.gt.maxn) stop '*** too many points, increase maxn' c c read gps data c ------------- c write(*,*) write(*,*) '- reading GPS data' ngps = -1 do i = 1,nn xgps(i) = 9.99d9 ygps(i) = 9.99d9 hgps(i) = 9.99d9 xdif(i) = 9.99d9 ydif(i) = 9.99d9 hdif(i) = 9.99d9 cosfi(i) = 9.99d9 lgap(i) = .true. enddo if (ltrim) read(10,*) 40 if (ltrim) then read(10,*,end=49) it,rfi,rla,rh,rsec else read(10,*,end=49) rsec,rfi,rla,rh endif c c 3 lines added to handle midnight (avo) if (ngps.eq.-1) nday0 = int(rsec/86400) rsec = rsec - 86400.0d0*nday0 tdec = rsec/3600.0d0 c if (ngps.eq.-1) then write(*,41) tdec, rsec 41 format(' GPS data start (dechr and sec): ',f9.4,f11.2) ngps = 0 endif ngps = ngps+1 if (abs(nint(tdec*3600)-rsec).gt.1) .write(*,*) 'warning: GPS time inconsistency sec = ',it c tdec = tdec - tshift c if (tdec.lt.tstart-9.9d-9) goto 40 i = nint((tdec-tstart)*3600)+1 if (i.gt.nn) goto 49 ygps(i) = rfi xgps(i) = rla hgps(i) = rh lgap(i) = .false. c goto 40 c 49 write(*,*) 'no of gps data read: ',ngps c c read ins data - linear interpolation to GPS time epochs c ------------------------------------------------------- c write(*,*) write(*,*) '- reading ins data' nins = 1 if (lvzint) then read(11,*) thr,rlatp,rlonp,hp,pitch,roll,hdg,hvz hp = hvz else read(11,*) thr,rlatp,rlonp,hp,pitch,roll,hdg endif rp = (thr-tstart)*3600+1 ip = rp c 50 if (lvzint) then read(11,*,end=55) thr,rlat,rlon,r,pitch,roll,hdg,h else read(11,*,end=55) thr,rlat,rlon,h,pitch,roll,hdg endif r = (thr-tstart)*3600+1 i = r dt = r - rp if (nins.gt.1.and.thr.gt.tstart) then if (thrp.ge.thr) then write(*,51) thr 51 format(' *** warning, EGI time reversal at:',f8.4) goto 54 endif if (abs(thrp-thr)*3600.gt.1.1) then write(*,52) thr,abs(thrp-thr)*3600 52 format(' *** warning, EGI time gap at:',f8.4,', sec: ',f8.2) goto 54 endif endif c c make differences INS-GPS into ydif,xdif,hdif c set egi values to zero in gps forcing case c if (ip.lt.i.and.ip.ge.1.and.i.le.nn.and.(.not.lgps)) then if (dt.le.0) stop '*** dt in INS file zero' yint = ((r-i)*rlatp + (i-rp)*rlat)/dt xint = ((r-i)*rlonp + (i-rp)*rlon)/dt hint = ((r-i)*hp + (i-rp)*h)/dt if (ygps(i).lt.9e9) ydif(i) = yint - ygps(i) if (ygps(i).lt.9e9) cosfi(i) = cos(ygps(i)/radeg) if (xgps(i).lt.9e9) xdif(i) = xint - xgps(i) if (hgps(i).lt.9e9) then hdif(i) = hint - hgps(i) hsum2 = hsum2+hdif(i)**2 nsum2 = nsum2+1 endif endif c c write insdata on temp file to speed up reread c if (ip.ge.1.and.i.le.nn) .write(30) thr,rlat,rlon,h,pitch,roll,hdg nins = nins+1 c 54 thrp = thr rlatp = rlat rlonp = rlon hp = h rp = r ip = i goto 50 c 55 write(*,*) .'number of ins data read in wanted time interval: ',nins rewind(30) c c now fill-up differences with linear interpolation c or interpolate for pure gps output of h c ------------------------------------------------- c if (lgps) then call rfill(ygps,nn,nfill,igapmax) call rfill(xgps,nn,nfill,igapmax) call rfill(hgps,nn,nfill,igapmax) if (nfill.gt.0) write(*,56) nfill,igapmax else call rfill(ydif,nn, nfill,igapmax) call rfill(xdif,nn, nfill,igapmax) call rfill(hdif,nn, nfill,igapmax) call rfill(cosfi,nn,nfill,igapmax) if (nfill.gt.0) write(*,56) nfill,igapmax 56 format(/' No of interpolated values in gps file:',i6, . ', max gap:',i5, ' sec ') c if (tc.gt.0) then call rc(nn,ydif,xx,tc,1.d0,1,1) call rc(nn,xx,ydif,tc,1.d0,1,-1) call rc(nn,xdif,xx,tc,1.d0,1,1) call rc(nn,xx,xdif,tc,1.d0,1,-1) call rc(nn,hdif,xx,tc,1.d0,1,1) call rc(nn,xx,hdif,tc,1.d0,1,-1) endif c c output differences c do i = 1, nn thr = tstart + (i-1)/3600.d0 dy = ydif(i)*degm dx = xdif(i)*cosfi(i)*degm write(21,57) thr,dy,dx,hdif(i) 57 format(f11.7,3f12.3) r = sqrt(dx**2+dy**2) if (r.gt.posmax) then posmax = r tposmax = (i-1)/3600.d0 endif enddo endif c c now read ins data again and apply interpolated GPS correction c again set egi values to zero if gps forcing c ------------------------------------------------------------- c 60 read(30,end=69) thr,rlat,rlon,h,pitch,roll,hdg r = (thr-tstart)*3600+1 if (r.lt.1) then i = 1 r = 0 elseif (r.ge.nn) then i = nn-1 r = 1 else i = r r = r-i endif c if (lgps) then rlat = (1-r)*ygps(i)+r*ygps(i+1) rlon = (1-r)*xgps(i)+r*xgps(i+1) h = (1-r)*hgps(i)+r*hgps(i+1) write(20,65) thr,rlat,rlon,h,pitch,roll,hdg else dlat = (1-r)*ydif(i)+r*ydif(i+1) dlon = (1-r)*xdif(i)+r*xdif(i+1) dh = (1-r)*hdif(i)+r*hdif(i+1) igap = 0 if (lgap(i).or.lgap(i+1)) igap = 1 write(20,65) thr,rlat-dlat,rlon-dlon,h-dh,pitch,roll,hdg,igap 65 format(' ',f10.7,f12.7,f13.7,f9.2,2f8.3,f9.3,i3) endif goto 60 c 69 if (lgps) then write(*,*) . 'gpsegi.pos written, coor from GPS and pitch,roll,hdg from EGI' else if (tposmax.gt.0) write(*,70) posmax,posmax/tposmax/1852 70 format(' Maximum position error:',f8.1,' m,',f8.2,' nm/hr') if (nsum2.gt.0) write(*,71) sqrt(hsum2/nsum2) 71 format(' R.m.s. height difference INS-GPS:',f9.2) endif end c subroutine rc(n,x,y,tc,dt,m,idir) cccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c c R C c c rc filter iir operator c tc = filter time const. c dt = data sampling interval c m = number of stages c idir = 1: forward c -1: backward c c dma/ds, modified jan 1992, rf c cccccccccccccccccccccccccccccccccccccccccccccccccccccccccc implicit double precision(a-h,o-z) dimension x(*),y(*) a = dt/(2*tc) b = (1.d0-a)/(1+a) c = a/(1+a) do 1 i=1,n 1 y(i)=x(i) if (idir.eq.1) then i1 = 1 i2 = n i3 = 1 else i1 = n i2 = 1 i3 = -1 endif c do 3 k = 1,m a = y(i1) d = a do 2 i = i1+i3,i2,i3 e = b*a + c*(y(i)+d) d = y(i) y(i) = e 2 a = e 3 continue end c subroutine rfill(x, n, nfill, igapmax) ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c c r f i l l c c fills array with holes with interpolated values c a hole is defined as a value .ge. 9.98d9 c c (c) rf/kms sept 96 c cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc implicit real*8(a-h,o-z) dimension x(*) dimension xx(28801) nfill = 0 igapmax = 0 do 10 i = 1, n if (x(i).le.9.98d9) then xx(i) = x(i) else nfill = nfill+1 j1 = i 5 j1 = j1-1 if (j1.lt.1) goto 6 if (x(j1).ge.9.98d9) goto 5 6 j2 = i 7 j2 = j2+1 if (j2.gt.n) goto 8 if (x(j2).ge.9.98d9) goto 7 8 if (j1.lt.1) then xx(i) = x(j2) ii = j2 elseif (j2.gt.n) then xx(i) = x(j1) ii = n-j1 else xx(i) = ((j2-i)*x(j1)+(i-j1)*x(j2))/(j2-j1) ii = j2-j1-1 endif if (ii.gt.igapmax) igapmax = ii endif 10 continue do 20 i = 1,n 20 x(i) = xx(i) return end