421 lines
14 KiB
Fortran
421 lines
14 KiB
Fortran
c****************************************************************
|
|
|
|
subroutine get_peg_info
|
|
|
|
c****************************************************************
|
|
c**
|
|
c** FILE NAME: get_peg_info.f
|
|
c**
|
|
c** DATE WRITTEN: 6/10/95
|
|
c**
|
|
c** PROGRAMMER: Scott Hensley
|
|
c**
|
|
c** FUNCTIONAL DESCRIPTION: This program reads simple emphemeris
|
|
c** information and compute the appropriate peg frame as well as
|
|
c** generating
|
|
c**
|
|
c** ROUTINES CALLED:none
|
|
c**
|
|
c** NOTES: none
|
|
c**
|
|
c** UPDATE LOG:
|
|
c** changed ERR= to END= for orbit reads and decremented count EJF 2001/1/18
|
|
c*****************************************************************
|
|
use get_peg_infoState
|
|
implicit none
|
|
|
|
c PARAMETER STATEMENTS:
|
|
|
|
character*20000 MESSAGE
|
|
real*8 r_awgs84,r_e2wgs84
|
|
parameter(r_awgs84=6378137.d0,r_e2wgs84=.00669437999015d0)
|
|
real*8 pi,r_dtor,r_rtod
|
|
parameter(pi=3.141592653589793d0) !if you have to ask, give it up
|
|
parameter(r_rtod=180.d0/pi,r_dtor=pi/180.d0) !radian to degree conversions
|
|
|
|
integer i_xyztollh,i_llhtoxyz
|
|
parameter(i_xyztollh=2, i_llhtoxyz=1)
|
|
integer i_schtoxyz,i_xyztosch
|
|
parameter(i_schtoxyz=0,i_xyztosch=1)
|
|
|
|
integer i_rdf,i_file
|
|
parameter(i_rdf=1,i_file=0)
|
|
|
|
integer MAXOBS
|
|
parameter(MAXOBS=20000)
|
|
|
|
integer i_orbitnum
|
|
parameter(i_orbitnum=75)
|
|
|
|
c INPUT VARIABLES:
|
|
|
|
c OUTPUT VARIABLES:
|
|
|
|
c LOCAL VARIABLES:
|
|
|
|
integer i,j,k
|
|
integer i_nd,i_ma,i_list(3)
|
|
real*8 r_time_scene_cen,r_dels
|
|
real*8 r_schvec(3),r_xyzvec(3),r_x(10),r_hfit(10),r_t,r_xyzvel(3)
|
|
real*8 r_xyzdot(3), r_schdot(3)
|
|
real*8 r_hdotfit(10),r_sdotfit(10),r_cdotfit(10),r_hffdot,r_cffdot,r_sffdot
|
|
real*8 r_cov(3,3),r_sig(10),r_chisq,r_hf,r_cff,r_cfit(10)
|
|
real*8 vertfit(3), horizfit(3), vertvfit(2), horizvfit(2)
|
|
real*8 r_earthgm, r_earthspindot
|
|
real*8 r_spinvec(3)
|
|
|
|
type ellipsoid
|
|
sequence
|
|
real*8 r_a
|
|
real*8 r_e2
|
|
end type ellipsoid
|
|
type (ellipsoid) elp
|
|
|
|
type peg_struct
|
|
sequence
|
|
real*8 r_lat
|
|
real*8 r_lon
|
|
real*8 r_hdg
|
|
end type peg_struct
|
|
type (peg_struct) peg
|
|
|
|
type pegtrans
|
|
sequence
|
|
real*8 r_mat(3,3)
|
|
real*8 r_matinv(3,3)
|
|
real*8 r_ov(3)
|
|
real*8 r_radcur
|
|
end type pegtrans
|
|
type (pegtrans) ptm
|
|
|
|
real*8 r_enumat(3,3),r_xyzenumat(3,3),r_enuvel(3)
|
|
real*8 r_xyzpeg(3),r_llhpeg(3)
|
|
real*8 r_tempv(3), r_tempa(3)
|
|
real*8 r_tempvec(3), r_inertialacc(3), r_bodyacc(3)
|
|
real*8 r_xyznorm, r_platsch(3)
|
|
real*8 r_smin(2),r_smax(2),r_sref
|
|
real*8 r_schvec1(3),r_xyzschmat(3,3),r_schxyzmat(3,3)
|
|
real*8 r_xyzvec1(3),r_velnorm,r_delsint,r_scale
|
|
real*8 r_endtimeslc,r_xyzvec11(3),r_schvec11(3)
|
|
|
|
real*8, allocatable, dimension(:,:) :: r_llh1,r_sch1
|
|
real*8, allocatable, dimension(:) :: r_hdg1,r_s1
|
|
|
|
|
|
c OUTPUT VARIABLES:
|
|
|
|
c DATA STATEMENTS:
|
|
|
|
|
|
data r_earthspindot /7.29211573052d-5/
|
|
data r_earthgm /3.98600448073d14/
|
|
|
|
c COMMON BLOCKS:
|
|
|
|
c SAVE STATEMENTS:
|
|
|
|
C FUNCTION STATEMENTS:
|
|
|
|
!Allocate the array that use to have MAXOBS size. Now use the actual value i_numobs
|
|
allocate(r_llh1(3,i_numobs))
|
|
allocate(r_sch1(3,i_numobs))
|
|
allocate(r_hdg1(i_numobs))
|
|
allocate(r_s1(i_numobs))
|
|
|
|
c PROCESSING STEPS:
|
|
|
|
|
|
elp%r_a = r_awgs84
|
|
elp%r_e2 = r_e2wgs84
|
|
|
|
|
|
c Convert the position data to lat,lon and find the heading for each point
|
|
|
|
|
|
c write(6,*) ' '
|
|
|
|
call writeStdOut('Transforming data orbit')
|
|
do k=1,i_numobs
|
|
|
|
c convert to lat,lon
|
|
|
|
call latlon(elp,r_xyz1(1,k),r_llh1(1,k),i_xyztollh)
|
|
|
|
c convert velocity to ENU frame
|
|
|
|
call enubasis(r_llh1(1,k),r_llh1(2,k),r_enumat)
|
|
call tranmat(r_enumat,r_xyzenumat)
|
|
|
|
c determine the heading
|
|
|
|
call matvec(r_xyzenumat,r_vxyz1(1,k),r_enuvel)
|
|
r_hdg1(k) = atan2(r_enuvel(1),r_enuvel(2))
|
|
write(MESSAGE,'(a,1x,i5)') 'Observation #: ',k
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f10.5,1x,f10.5,1x,f12.3)') 'Lat, Lon & Height: ',r_llh1(1,k)*r_rtod,r_llh1(2,k)*r_rtod,r_llh1(3,k)
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f15.7)') 'Heading: ',r_hdg1(k)*r_rtod
|
|
call writeStdOut(MESSAGE)
|
|
enddo !observations
|
|
|
|
|
|
c determine the peg lat,lon and heading to use - algorithm assumes a frame size scene and uses lat,lon at
|
|
c scene center and heading a scene center for peg coordinates. Orbit 1 is the master frame and peg is determined
|
|
c using it's orbit only. It is rough but should suffice for most applications.
|
|
|
|
r_time_first_line = r_timeslc + (i_startline-1)/r_prf !time to first line in Interferogram
|
|
r_time_scene_cen = r_time_first_line + (i_numlines*i_looksaz)/(2.d0*r_prf)
|
|
|
|
call writeStdOut(' << Output Data >> ')
|
|
write(MESSAGE,'(a,1x,2(f12.3,1x))') 'Time to first/middle scene: ',r_time_first_line,
|
|
+ r_time_scene_cen
|
|
call writeStdOut(MESSAGE)
|
|
|
|
c interpolate the motion data to the scene center using a quadratic interpolator
|
|
|
|
call inter_motion(r_time,r_xyz1,i_numobs,r_time_scene_cen,r_xyzpeg)
|
|
call inter_motion(r_time,r_vxyz1,i_numobs,r_time_scene_cen,r_vxyzpeg)
|
|
write(MESSAGE,'(a,1x,3(f12.3,1x))') 'Pos Peg = ',(r_xyzpeg(j),j=1,3)
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,3(f12.6,1x))') 'Vel Peg = ',(r_vxyzpeg(j),j=1,3)
|
|
call writeStdOut(MESSAGE)
|
|
call norm(r_vxyzpeg,r_velnorm)
|
|
|
|
c take the lat,lon as the peg point and the heading as the peg heading
|
|
|
|
call latlon(elp,r_xyzpeg,r_llhpeg,i_xyztollh)
|
|
call enubasis(r_llhpeg(1),r_llhpeg(2),r_enumat)
|
|
call tranmat(r_enumat,r_xyzenumat)
|
|
call matvec(r_xyzenumat,r_vxyzpeg,r_enuvel)
|
|
peg%r_hdg = atan2(r_enuvel(1),r_enuvel(2))
|
|
|
|
peg%r_lat = r_llhpeg(1)
|
|
peg%r_lon = r_llhpeg(2)
|
|
|
|
call radar_to_xyz(elp,peg,ptm)
|
|
|
|
r_pegLat = peg%r_lat*r_rtod
|
|
r_pegLon = peg%r_lon*r_rtod
|
|
r_pegHgt = r_llhpeg(3)
|
|
r_pegHead = peg%r_hdg*r_rtod
|
|
write(MESSAGE,'(a,1x,f12.7,1x,f12.7,1x,f12.3)') 'Peg Lat/Lon , H = ',
|
|
+ peg%r_lat*r_rtod,peg%r_lon*r_rtod,r_llhpeg(3)
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f15.7)') 'Peg Heading = ',peg%r_hdg*r_rtod
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f15.5)') 'Radius Curvature = ',ptm%r_radcur
|
|
call writeStdOut(MESSAGE)
|
|
|
|
call writeStdOut('Rotation matrix ')
|
|
write(MESSAGE,905) ' First row = ',ptm%r_mat(1,1),ptm%r_mat(1,2),ptm%r_mat(1,3)
|
|
905 format(a,1x,3(f12.9,1x))
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,905) ' Second row = ',ptm%r_mat(2,1),ptm%r_mat(2,2),ptm%r_mat(2,3)
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,905) ' Third row = ',ptm%r_mat(3,1),ptm%r_mat(3,2),ptm%r_mat(3,3)
|
|
call writeStdOut(MESSAGE)
|
|
call writeStdOut('Translation vector ')
|
|
write(MESSAGE,906) ' Vector = ',ptm%r_ov
|
|
906 format(a,1x,3(f14.5,1x))
|
|
call writeStdOut(MESSAGE)
|
|
|
|
r_spinvec(1) = 0.
|
|
r_spinvec(2) = 0.
|
|
r_spinvec(3) = r_spindot
|
|
|
|
call norm(r_xyzpeg,r_xyznorm)
|
|
|
|
call cross(r_spinvec,r_xyzpeg,r_tempv)
|
|
|
|
do k=1,3
|
|
r_inertialacc(k) = -(r_gm*r_xyzpeg(k))/r_xyznorm**3
|
|
enddo
|
|
|
|
call cross(r_spinvec,r_vxyzpeg,r_tempa)
|
|
call cross(r_spinvec,r_tempv,r_tempvec)
|
|
|
|
do k=1,3
|
|
r_bodyacc(k) = r_inertialacc(k) - 2.d0*r_tempa(k) - r_tempvec(k)
|
|
enddo
|
|
|
|
c convert back to a local SCH basis
|
|
|
|
call convert_sch_to_xyz(ptm,r_platsch,r_xyzpeg,i_xyztosch)
|
|
call schbasis(ptm,r_platsch,r_xyzschmat,r_schxyzmat)
|
|
call matvec(r_xyzschmat,r_bodyacc,r_platacc)
|
|
call matvec(r_xyzschmat,r_vxyzpeg,r_platvel)
|
|
|
|
write(MESSAGE,'(a,x,3(f15.7,x))') 'Platform SCH Velocity (m/s): ',r_platvel
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,x,3(f15.7,x))') 'Platform SCH Acceleration (m/s^2): ',r_platacc
|
|
call writeStdOut(MESSAGE)
|
|
|
|
|
|
c compute delta S on ground and in Orbit for SLC and Interferogram
|
|
|
|
r_dels = r_platvel(1)/r_prf
|
|
r_scale = ptm%r_radcur/(r_llhpeg(3) + ptm%r_radcur)
|
|
|
|
r_delsint = r_dels*i_looksaz
|
|
|
|
call writeStdOut(' SLC Interferogram')
|
|
write(MESSAGE,'(a,1x,f10.5,1x,f10.5)') 'Delta S on Ground: ',r_dels*r_scale,r_delsint*r_scale
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f10.5,1x,f10.5)') 'Delta S in Orbit: ',r_dels,r_delsint
|
|
call writeStdOut(MESSAGE)
|
|
|
|
c convert the motion data to SCH coordinates
|
|
|
|
|
|
call writeStdOut('Transforming data orbit: ')
|
|
call writeStdOut('SCH positions ')
|
|
|
|
r_smin(i) = 1.d25
|
|
r_smax(i) = -1.d25
|
|
|
|
do k=1,i_numobs
|
|
|
|
call convert_sch_to_xyz(ptm,r_sch1(1,k),r_xyz1(1,k),i_xyztosch)
|
|
write(MESSAGE,'(a,1x,3(f15.3,1x))') 'SCH : ',r_sch1(1,k),r_sch1(2,k),r_sch1(3,k)
|
|
call writeStdOut(MESSAGE)
|
|
r_smin(1) = min(r_smin(1),r_sch1(1,k))
|
|
r_smax(1) = max(r_smax(1),r_sch1(1,k))
|
|
r_s1(k) = r_sch1(1,k)
|
|
|
|
enddo !observations
|
|
|
|
|
|
c compute the starting S coordinate for two scenes - and ending S coordinates
|
|
|
|
r_endtimeslc = r_timeslc + i_slclines/r_prf
|
|
call inter_motion(r_time,r_xyz1,i_numobs,r_timeslc,r_xyzvec1)
|
|
call convert_sch_to_xyz(ptm,r_schvec1,r_xyzvec1,i_xyztosch)
|
|
call inter_motion(r_time,r_xyz1,i_numobs,r_endtimeslc,r_xyzvec11)
|
|
call convert_sch_to_xyz(ptm,r_schvec11,r_xyzvec11,i_xyztosch)
|
|
|
|
r_sref = r_schvec1(1)
|
|
|
|
c write out region of intersection if two orbits, and min,max x ccordinates
|
|
|
|
write(MESSAGE,'(a,1x,f15.3,1x,f15.3)') 'Min, Max S for orbit 1: ',r_smin(1),r_smax(1)
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f15.3,1x,f15.3)') 'Min, Max S for orbit 1 II: ',r_schvec1(1),r_schvec11(1)
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f15.3)') 'Reference S for fits: ',r_sref
|
|
call writeStdOut(MESSAGE)
|
|
c fit the height data to a quadratic for use in inverse3d
|
|
|
|
call writeStdOut(' SCH Positions for 10 points along track ')
|
|
|
|
do i=1,10
|
|
|
|
r_t = r_time_first_line + (i_looksaz/r_prf)*((float(i_numlines)/9.d0)*(i-1))
|
|
c r_x(i) = ((float(i_numlines)/(9.d0*i_looksaz))*(i-1) -
|
|
c + float(i_numlines)/(2.d0*i_looksaz))*(r_dels*i_looksaz)
|
|
call inter_motion(r_time,r_xyz1,i_numobs,r_t,r_xyzvec)
|
|
call inter_motion(r_time,r_vxyz1,i_numobs,r_t,r_xyzdot)
|
|
call convert_sch_to_xyz(ptm,r_schvec,r_xyzvec,i_xyztosch)
|
|
call convert_schdot_to_xyzdot(ptm,r_schvec,r_xyzvec,r_schdot,r_xyzdot,i_xyztosch)
|
|
|
|
r_hfit(i) = r_schvec(3)
|
|
r_cfit(i) = r_schvec(2)
|
|
|
|
r_hdotfit(i) = r_schdot(3)
|
|
r_cdotfit(i) = r_schdot(2)
|
|
r_sdotfit(i) = r_schdot(1)
|
|
|
|
r_x(i) = r_schvec(1) - r_sref
|
|
r_sig(i) = 1.d0
|
|
|
|
write(MESSAGE,'(a,1x,f10.2,1x,3(f12.3,1x))') 'Time/Pos: ',r_t,r_schvec
|
|
call writeStdOut(MESSAGE)
|
|
|
|
enddo
|
|
|
|
c fit orbit one C,H values to a quadratic
|
|
|
|
i_nd = 10
|
|
i_ma = 3
|
|
i_list(1) = 1
|
|
i_list(2) = 2
|
|
i_list(3) = 3
|
|
call lfit(r_x,r_hfit,r_sig,i_nd,r_af,i_ma,i_list,i_ma,r_cov,i_ma,r_chisq)
|
|
call lfit(r_x,r_cfit,r_sig,i_nd,r_cf,i_ma,i_list,i_ma,r_cov,i_ma,r_chisq)
|
|
vertfit = r_af
|
|
horizfit = r_cf
|
|
call writeStdOut(' * Quadratic Fit Coefficients for Height/Cross Track * ')
|
|
write(MESSAGE,'(a,1x,3(e20.10,1x))') 'Vertical Fit: ',r_af
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,3(e20.10,1x))') 'Horizontal Fit: ',r_cf
|
|
call writeStdOut(MESSAGE)
|
|
do i=1,10
|
|
r_hf = r_af(1) + r_x(i)*(r_af(2) + r_x(i)*r_af(3))
|
|
r_cff = r_cf(1) + r_x(i)*(r_cf(2) + r_x(i)*r_cf(3))
|
|
write(MESSAGE,'(a,1x,f12.2,1x,f12.2,1x,f12.6)') 'Fit check h: ',r_hfit(i),r_hf,r_hf-r_hfit(i)
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f12.2,1x,f12.2,1x,f12.6)') 'Fit check c: ',r_cfit(i),r_cff,r_cff-r_cfit(i)
|
|
call writeStdOut(MESSAGE)
|
|
enddo
|
|
|
|
c fit orbit one Cdot,Hdot values to a line
|
|
|
|
i_nd = 10
|
|
i_ma = 2
|
|
i_list(1) = 1
|
|
i_list(2) = 2
|
|
call lfit(r_x,r_sdotfit,r_sig,i_nd,r_sfdot,i_ma,i_list,i_ma,r_cov,i_ma,r_chisq)
|
|
call lfit(r_x,r_hdotfit,r_sig,i_nd,r_afdot,i_ma,i_list,i_ma,r_cov,i_ma,r_chisq)
|
|
call lfit(r_x,r_cdotfit,r_sig,i_nd,r_cfdot,i_ma,i_list,i_ma,r_cov,i_ma,r_chisq)
|
|
vertvfit = r_afdot
|
|
horizvfit = r_cfdot
|
|
call writeStdOut(' * Linear Fit Coefficients for Height/Cross-Track/Along-Track * ')
|
|
write(MESSAGE,'(a,1x,3(e20.10,1x))') 'Vertical Velocity Fit: ',r_afdot
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,3(e20.10,1x))') 'Cross-Track Velocity Fit: ',r_cfdot
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,3(e20.10,1x))') 'Along-Track Velocity Fit: ',r_sfdot
|
|
call writeStdOut(MESSAGE)
|
|
do i=1,10
|
|
r_hffdot = r_afdot(1) + r_x(i)*r_afdot(2)
|
|
r_cffdot = r_cfdot(1) + r_x(i)*r_cfdot(2)
|
|
r_sffdot = r_sfdot(1) + r_x(i)*r_cfdot(2)
|
|
write(MESSAGE,'(a,1x,f12.2,1x,f12.2,1x,f12.6)') 'Fit check H: ',r_hdotfit(i),r_hffdot,r_hffdot-r_hdotfit(i)
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f12.2,1x,f12.2,1x,f12.6)') 'Fit check C: ',r_cdotfit(i),r_cffdot,r_cffdot-r_cdotfit(i)
|
|
call writeStdOut(MESSAGE)
|
|
write(MESSAGE,'(a,1x,f12.2,1x,f12.2,1x,f12.6)') 'Fit check S: ',r_sdotfit(i),r_sffdot,r_sffdot-r_cdotfit(i)
|
|
call writeStdOut(MESSAGE)
|
|
enddo
|
|
|
|
do i=1,i_numlines
|
|
r_t = r_time_first_line + (1.d0/r_prf)*(i-1)*i_looksaz
|
|
call inter_motion(r_time,r_xyz1,i_numobs,r_t,r_xyzvec)
|
|
call inter_motion(r_time,r_vxyz1,i_numobs,r_t,r_xyzvel)
|
|
r_intPos(i,:) = r_xyzvec(:)
|
|
r_intVel(i,:) = r_xyzvel(:)
|
|
enddo
|
|
|
|
r_pegRadius = ptm%r_radcur
|
|
r_grndSpace = r_delsint*r_scale
|
|
r_transVect = ptm%r_ov
|
|
r_transfMat = ptm%r_mat
|
|
|
|
end
|
|
|
|
|
|
c***********************************************************************
|
|
|
|
subroutine funcs(x,p,np)
|
|
|
|
real*8 x
|
|
real*8 p(np)
|
|
|
|
p(1) = 1.
|
|
do j=2,np
|
|
p(j) = p(j-1)*x
|
|
enddo
|
|
return
|
|
end
|
|
|
|
|