!c23456789012345678901234567890123456789012345678901234567890123456789012 !c !c compiled on moka with the command: /usr/bin/gfortran geolocate.f !c !c r_pos, r_range and r_a are in meters !c r_vel is in m/s !c r_squint is in radians and is for a right-looking SAR !c r_llh consists of lat and lon in radians and height in meters !c r_look_angle is in radians !c !c23456789012345678901234567890123456789012345678901234567890123456789012 subroutine geolocate(r_pos, r_vel, r_range, r_squint, ip_side, r_a, r_e2, & r_llh, r_look_angle,r_incidence_angle) implicit none ccccc declare parameters integer*4 i_schtoxyz parameter (i_schtoxyz = 0) integer*4 i_xyztollh parameter (i_xyztollh = 2) integer*4 i_xyztosch parameter (i_xyztosch = 1) ccccc declare functions real*8 rdir ccccc declare variables integer*4 i_i integer*4 i_side, ip_side ! sat lk direction (+1 right lk, -1 left lk) real*8 r_a real*8 r_a_dum real*8 r_b real*8 r_bias(3) real*8 r_cosg real*8 r_e2 real*8 r_e2_dum real*8 r_enubias(3) real*8 r_enumat(3,3) real*8 r_enuvel(3) real*8 r_img_pln_rad real*8 r_incidence_angle real*8 r_lat real*8 r_lk_xyz(3) real*8 r_lk_xyz_mag real*8 r_llh(3) real*8 r_lon real*8 r_look_angle real*8 r_m real*8 r_mag real*8 r_pos(3) real*8 r_rad real*8 r_range real*8 r_sch2(3) real*8 r_sing real*8 r_sinm real*8 r_squint real*8 r_tanm real*8 r_target_d real*8 r_vel(3) real*8 r_xyz(3) real*8 sc_az_nom real*8 sc_d real*8 sc_h real*8 sc_hdg real*8 sc_lat real*8 sc_lon real*8 sc_r real*8 sc_sch(3) real*8 sc_vel real*8 u_lk(3) real*8 u_lk_xyz(3) real*8 u_n(3) real*8 xyz2enu(3,3) ccccc declare derived data types type ellipsoid sequence real (8) r_a real (8) r_e2 end type ellipsoid type (ellipsoid) elp type pegtype sequence real (8) r_lat real (8) r_lon real (8) r_hdg end type pegtype type (pegtype) 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) ptm2 ccccc common statements c r_a_dum = r_a c r_e2_dum = r_e2 common /ellipsoid/ r_a_dum, r_e2_dum ccccc data statements data r_bias /0.061d3,-0.285d3,-0.181d3/ real*8, parameter :: r_dtor = atan(1.d0) / 45.d0 ccccc initialize !! i_side = +1 ! right looking i_side = -1*ip_side !ISCE convention to code convention elp%r_a = r_a elp%r_e2 = r_e2 r_a_dum = r_a r_e2_dum = r_e2 sc_az_nom = dble(i_side) * (90.d0 * r_dtor - r_squint) r_b = sqrt(r_a**2 * (1.d0 - r_e2)) ccccc determine spacecraft info call norm(r_pos,sc_r) call norm(r_vel,sc_vel) call latlon(elp,r_pos,r_llh,i_xyztollh) sc_lat = r_llh(1) sc_lon = r_llh(2) sc_h = r_llh(3) call enubasis(sc_lat,sc_lon,r_enumat) call tranmat(r_enumat,xyz2enu) call matvec(r_enumat,r_bias,r_enubias) call matvec(xyz2enu,r_vel,r_enuvel) sc_hdg = atan2(r_enuvel(1),r_enuvel(2)) ccccc solve law of cosines to determine lk angle to reference ellipsoid peg%r_lat = sc_lat peg%r_lon = sc_lon peg%r_hdg = sc_hdg+sc_az_nom r_img_pln_rad = rdir(r_a,r_e2,sc_hdg+sc_az_nom,sc_lat) call radar_to_xyz(elp,peg,ptm2) call convert_sch_to_xyz(ptm2,sc_sch,r_pos,i_xyztosch) r_target_d = r_img_pln_rad sc_d = r_img_pln_rad + sc_sch(3) r_look_angle = acos((sc_d**2 + r_range**2 - r_target_d**2) / & (2.d0 * sc_d * r_range)) ccccc construct look vector (in SCH coord.) from computed look angle u_lk(1) = +sin(r_look_angle) u_lk(2) = 0.d0 u_lk(3) = -cos(r_look_angle) ccccc compute xyz vector from earth center to ellipsoid do i_i = 1 , 3 r_xyz(i_i) = sc_sch(i_i) + u_lk(i_i) * r_range enddo r_m = sqrt(r_xyz(1)**2 + r_xyz(2)**2) r_tanm = r_m / (r_img_pln_rad+r_xyz(3)) r_sinm = r_m / (r_m**2+(r_img_pln_rad+r_xyz(3))**2) r_cosg = r_xyz(1) / r_m r_sing = r_xyz(2) / r_m r_sch2(1) = r_img_pln_rad * atan(r_tanm * r_cosg) r_sch2(2) = r_img_pln_rad * asin(r_sinm * r_sing) r_sch2(3) = sqrt((r_img_pln_rad + r_xyz(3))**2 + r_m**2) - & r_img_pln_rad call convert_sch_to_xyz(ptm2,r_sch2,r_xyz,i_schtoxyz) call latlon(elp,r_pos,r_llh,i_xyztollh) r_lat = r_llh(1) r_lon = r_llh(2) r_rad = r_llh(3) ccccc compute lat, lon and hgt call latlon(elp,r_xyz,r_llh,i_xyztollh) ccccc compute ellipsoid outward unit surface normal r_mag = 2.d0 * sqrt((r_xyz(1)/r_a**2)**2 + & (r_xyz(2)/r_a**2)**2 + & (r_xyz(3)/r_b**2)**2) u_n(1) = (2.d0 * r_xyz(1) / r_a**2) / r_mag u_n(2) = (2.d0 * r_xyz(2) / r_a**2) / r_mag u_n(3) = (2.d0 * r_xyz(3) / r_b**2) / r_mag ccccc compute unit look vector in cartesian coordinates do i_i = 1 , 3 r_lk_xyz(i_i) = r_xyz(i_i) - r_pos(i_i) enddo r_lk_xyz_mag = sqrt(r_lk_xyz(1)**2+r_lk_xyz(2)**2+r_lk_xyz(3)**2) do i_i = 1 , 3 u_lk_xyz(i_i) = r_lk_xyz(i_i) / r_lk_xyz_mag enddo ccccc compute incidence angle r_incidence_angle = acos(-u_n(1)*u_lk_xyz(1)-u_n(2)*u_lk_xyz(2)- & u_n(3)*u_lk_xyz(3)) ccccc write results c write (*,*) 'r_look_angle (deg): ' , r_look_angle / r_dtor c write (*,*) return end c23456789012345678901234567890123456789012345678901234567890123456789012 c**************************************************************** subroutine norm(r_v,r_n) c**************************************************************** c** c** FILE NAME: norm.f c** c** DATE WRITTEN: 8/3/90 c** c** PROGRAMMER:Scott Hensley c** c** FUNCTIONAL DESCRIPTION: The subroutine takes vector and returns c** its norm. c** c** ROUTINES CALLED:none c** c** NOTES: none c** c** UPDATE LOG: c** c***************************************************************** implicit none c INPUT VARIABLES: real*8 r_v(3) !3x1 vector c OUTPUT VARIABLES:see input c LOCAL VARIABLES: real*8 r_n c PROCESSING STEPS: c compute vector norm r_n = sqrt(r_v(1)**2 + r_v(2)**2 + r_v(3)**2) end c23456789012345678901234567890123456789012345678901234567890123456789012 c**************************************************************** subroutine latlon(elp,r_v,r_llh,i_type) c**************************************************************** c** c** FILE NAME: latlon.f c** c** DATE WRITTEN:7/22/93 c** c** PROGRAMMER:Scott Hensley c** c** FUNCTIONAL DESCRIPTION:This program converts a vector to c** lat,lon and height above the reference ellipsoid or given a c** lat,lon and height produces a geocentric vector. c** c** ROUTINES CALLED:none c** c** NOTES: none c** c** UPDATE LOG: c** c**************************************************************** implicit none c INPUT VARIABLES: integer i_type !1=lat,lon to vector,2= vector to lat,lon c structure /ellipsoid/ c real*8 r_a c real*8 r_e2 c end structure c record /ellipsoid/ elp type ellipsoid sequence real (8) r_a real (8) r_e2 end type ellipsoid type (ellipsoid) elp real*8 r_v(3) !geocentric vector (meters) real*8 r_llh(3) !lat (deg -90 to 90),lon (deg -180 to 180),hgt c OUTPUT VARIABLES: see input c LOCAL VARIABLES: real*8 r_re,r_q2,r_q3,r_b,r_q real*8 r_p,r_tant,r_theta,r_a,r_e2 c DATA STATEMENTS: C FUNCTION STATEMENTS: c PROCESSING STEPS: r_a = elp%r_a r_e2 = elp%r_e2 if(i_type .eq. 1)then !convert lat,lon to vector r_re = r_a/sqrt(1.d0 - r_e2*sin(r_llh(1))**2) r_v(1) = (r_re + r_llh(3))*cos(r_llh(1))*cos(r_llh(2)) r_v(2) = (r_re + r_llh(3))*cos(r_llh(1))*sin(r_llh(2)) r_v(3) = (r_re*(1.d0-r_e2) + r_llh(3))*sin(r_llh(1)) elseif(i_type .eq. 2)then !convert vector to lat,lon r_q2 = 1.d0/(1.d0 - r_e2) r_q = sqrt(r_q2) r_q3 = r_q2 - 1.d0 r_b = r_a*sqrt(1.d0 - r_e2) r_llh(2) = atan2(r_v(2),r_v(1)) r_p = sqrt(r_v(1)**2 + r_v(2)**2) r_tant = (r_v(3)/r_p)*r_q r_theta = atan(r_tant) r_tant = (r_v(3) + r_q3*r_b*sin(r_theta)**3)/ + (r_p - r_e2*r_a*cos(r_theta)**3) r_llh(1) = atan(r_tant) r_re = r_a/sqrt(1.d0 - r_e2*sin(r_llh(1))**2) r_llh(3) = r_p/cos(r_llh(1)) - r_re endif end c23456789012345678901234567890123456789012345678901234567890123456789012 c**************************************************************** subroutine enubasis(r_lat,r_lon,r_enumat) c**************************************************************** c** c** FILE NAME: enubasis.f c** c** DATE WRITTEN: 7/22/93 c** c** PROGRAMMER:Scott Hensley c** c** FUNCTIONAL DESCRIPTION:Takes a lat and lon and returns a c** change of basis matrix from ENU to geocentric coordinates. c** c** ROUTINES CALLED:none c** c** NOTES: none c** c** UPDATE LOG: c**************************************************************** implicit none c INPUT VARIABLES: real*8 r_lat !latitude (deg) real*8 r_lon !longitude (deg) c OUTPUT VARIABLES: real*8 r_enumat(3,3) c LOCAL VARIABLES: real*8 r_slt,r_clt,r_clo,r_slo c DATA STATEMENTS: C FUNCTION STATEMENTS: c PROCESSING STEPS: r_clt = cos(r_lat) r_slt = sin(r_lat) r_clo = cos(r_lon) r_slo = sin(r_lon) c North vector r_enumat(1,2) = -r_slt*r_clo r_enumat(2,2) = -r_slt*r_slo r_enumat(3,2) = r_clt c East vector r_enumat(1,1) = -r_slo r_enumat(2,1) = r_clo r_enumat(3,1) = 0.d0 c Up vector r_enumat(1,3) = r_clt*r_clo r_enumat(2,3) = r_clt*r_slo r_enumat(3,3) = r_slt end c23456789012345678901234567890123456789012345678901234567890123456789012 c**************************************************************** subroutine tranmat(r_a,r_b) c**************************************************************** c** c** FILE NAME: tranmat.f c** c** DATE WRITTEN: 8/3/90 c** c** PROGRAMMER:Scott Hensley c** c** FUNCTIONAL DESCRIPTION: The subroutine takes a 3x3 matrix c** and computes its transpose. c** c** ROUTINES CALLED:none c** c** NOTES: none c** c** UPDATE LOG: c** c***************************************************************** implicit none c INPUT VARIABLES: real*8 r_a(3,3) !3x3 matrix c OUTPUT VARIABLES: real*8 r_b(3,3) !3x3 matrix c LOCAL VARIABLES: integer i,j c PROCESSING STEPS: c compute matrix product do i=1,3 do j=1,3 r_b(i,j) = r_a(j,i) enddo enddo end c23456789012345678901234567890123456789012345678901234567890123456789012 c**************************************************************** subroutine matvec(r_t,r_v,r_w) c**************************************************************** c** c** FILE NAME: matvec.f c** c** DATE WRITTEN: 7/20/90 c** c** PROGRAMMER:Scott Hensley c** c** FUNCTIONAL DESCRIPTION: The subroutine takes a 3x3 matrix c** and a 3x1 vector a multiplies them to return another 3x1 c** vector. c** c** ROUTINES CALLED:none c** c** NOTES: none c** c** UPDATE LOG: c** c***************************************************************** implicit none c INPUT VARIABLES: real*8 r_t(3,3) !3x3 matrix real*8 r_v(3) !3x1 vector c OUTPUT VARIABLES: real*8 r_w(3) !3x1 vector c LOCAL VARIABLES:none c PROCESSING STEPS: c compute matrix product r_w(1) = r_t(1,1)*r_v(1) + r_t(1,2)*r_v(2) + r_t(1,3)*r_v(3) r_w(2) = r_t(2,1)*r_v(1) + r_t(2,2)*r_v(2) + r_t(2,3)*r_v(3) r_w(3) = r_t(3,1)*r_v(1) + r_t(3,2)*r_v(2) + r_t(3,3)*r_v(3) end c23456789012345678901234567890123456789012345678901234567890123456789012 c**************************************************************** subroutine radar_to_xyz(elp,peg,ptm) c**************************************************************** c** c** FILE NAME: radar_to_xyz.for c** c** DATE WRITTEN:1/15/93 c** c** PROGRAMMER:Scott Hensley c** c** FUNCTIONAL DESCRIPTION:This routine computes the transformation c** matrix & translation vector needed to get between radar (s,c,h) c** coordinates and (x,y,z) WGS-84 coordinates. c** c** ROUTINES CALLED:euler, c** c** NOTES: none c** c** UPDATE LOG: c** c***************************************************************** implicit none c INPUT VARIABLES: c structure /ellipsoid/ c real*8 r_a c real*8 r_e2 c end structure c record /ellipsoid/ elp c structure /peg/ c real*8 r_lat c real*8 r_lon c real*8 r_hdg c end structure c record /peg/ peg type ellipsoid sequence real (8) r_a real (8) r_e2 end type ellipsoid type (ellipsoid) elp type pegtype sequence real (8) r_lat real (8) r_lon real (8) r_hdg end type pegtype type (pegtype) peg c OUTPUT VARIABLES: c structure /pegtrans/ c real*8 r_mat(3,3) c real*8 r_matinv(3,3) c real*8 r_ov(3) c real*8 r_radcur c end structure c record /pegtrans/ ptm 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 c LOCAL VARIABLES: integer i,j,i_type real*8 r_llh(3),r_p(3),r_slt,r_clt,r_clo,r_slo real*8 r_up(3),r_chg,r_shg,rdir c DATA STATEMENTS:none C FUNCTION STATEMENTS: external rdir c PROCESSING STEPS: c first determine the rotation matrix r_clt = cos(peg%r_lat) r_slt = sin(peg%r_lat) r_clo = cos(peg%r_lon) r_slo = sin(peg%r_lon) r_chg = cos(peg%r_hdg) r_shg = sin(peg%r_hdg) ptm%r_mat(1,1) = r_clt*r_clo ptm%r_mat(1,2) = -r_shg*r_slo - r_slt*r_clo*r_chg ptm%r_mat(1,3) = r_slo*r_chg - r_slt*r_clo*r_shg ptm%r_mat(2,1) = r_clt*r_slo ptm%r_mat(2,2) = r_clo*r_shg - r_slt*r_slo*r_chg ptm%r_mat(2,3) = -r_clo*r_chg - r_slt*r_slo*r_shg ptm%r_mat(3,1) = r_slt ptm%r_mat(3,2) = r_clt*r_chg ptm%r_mat(3,3) = r_clt*r_shg do i=1,3 do j=1,3 ptm%r_matinv(i,j) = ptm%r_mat(j,i) enddo enddo c find the translation vector ptm%r_radcur = rdir(elp%r_a,elp%r_e2,peg%r_hdg,peg%r_lat) i_type = 1 r_llh(1) = peg%r_lat r_llh(2) = peg%r_lon r_llh(3) = 0.0d0 call latlon(elp,r_p,r_llh,i_type) r_clt = cos(peg%r_lat) r_slt = sin(peg%r_lat) r_clo = cos(peg%r_lon) r_slo = sin(peg%r_lon) r_up(1) = r_clt*r_clo r_up(2) = r_clt*r_slo r_up(3) = r_slt do i=1,3 ptm%r_ov(i) = r_p(i) - ptm%r_radcur*r_up(i) enddo end c23456789012345678901234567890123456789012345678901234567890123456789012 c**************************************************************** subroutine convert_sch_to_xyz(ptm,r_schv,r_xyzv,i_type) c**************************************************************** c** c** FILE NAME: convert_sch_to_xyz.for c** c** DATE WRITTEN:1/15/93 c** c** PROGRAMMER:Scott Hensley c** c** FUNCTIONAL DESCRIPTION: This routine applies the affine matrix c** provided to convert the sch coordinates xyz WGS-84 coordintes or c** the inverse transformation. c** c** ROUTINES CALLED:latlon,matvec c** c** NOTES: none c** c** UPDATE LOG: c** c***************************************************************** implicit none c INPUT VARIABLES: c structure /pegtrans/ !transformation parameters c real*8 r_mat(3,3) c real*8 r_matinv(3,3) c real*8 r_ov(3) c real*8 r_radcur c end structure c record /pegtrans/ ptm 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_schv(3) !sch coordinates of a point real*8 r_xyzv(3) !WGS-84 coordinates of a point integer i_type !i_type = 0 sch => xyz ; !i_type = 1 xyz => sch c OUTPUT VARIABLES: see input c LOCAL VARIABLES: integer i_t real*8 r_schvt(3),r_llh(3) c structure /ellipsoid/ c real*8 r_a c real*8 r_e2 c end structure c record /ellipsoid/ sph type ellipsoid sequence real (8) r_a real (8) r_e2 end type ellipsoid type (ellipsoid) sph c DATA STATEMENTS: C FUNCTION STATEMENTS:none c PROCESSING STEPS: c compute the linear portion of the transformation sph%r_a = ptm%r_radcur sph%r_e2 = 0.0d0 if(i_type .eq. 0)then r_llh(1) = r_schv(2)/ptm%r_radcur r_llh(2) = r_schv(1)/ptm%r_radcur r_llh(3) = r_schv(3) i_t = 1 call latlon(sph,r_schvt,r_llh,i_t) call matvec(ptm%r_mat,r_schvt,r_xyzv) call lincomb(1.d0,r_xyzv,1.d0,ptm%r_ov,r_xyzv) elseif(i_type .eq. 1)then call lincomb(1.d0,r_xyzv,-1.d0,ptm%r_ov,r_schvt) call matvec(ptm%r_matinv,r_schvt,r_schv) i_t = 2 call latlon(sph,r_schv,r_llh,i_t) r_schv(1) = ptm%r_radcur*r_llh(2) r_schv(2) = ptm%r_radcur*r_llh(1) r_schv(3) = r_llh(3) endif end c23456789012345678901234567890123456789012345678901234567890123456789012 c**************************************************************** subroutine lincomb(r_k1,r_u,r_k2,r_v,r_w) c**************************************************************** c** c** FILE NAME: lincomb.f c** c** DATE WRITTEN: 8/3/90 c** c** PROGRAMMER:Scott Hensley c** c** FUNCTIONAL DESCRIPTION: The subroutine forms the linear c** combination of two vectors. c** c** ROUTINES CALLED:none c** c** NOTES: none c** c** UPDATE LOG: c** c***************************************************************** implicit none c INPUT VARIABLES: real*8 r_u(3) !3x1 vector real*8 r_v(3) !3x1 vector real*8 r_k1 !scalar real*8 r_k2 !scalar c OUTPUT VARIABLES: real*8 r_w(3) !3x1 vector c LOCAL VARIABLES:none c PROCESSING STEPS: c compute linear combination r_w(1) = r_k1*r_u(1) + r_k2*r_v(1) r_w(2) = r_k1*r_u(2) + r_k2*r_v(2) r_w(3) = r_k1*r_u(3) + r_k2*r_v(3) end c23456789012345678901234567890123456789012345678901234567890123456789012 c**************************************************************** c c Various curvature functions c c c**************************************************************** c** c** FILE NAME: curvature.f c** c** DATE WRITTEN: 12/02/93 c** c** PROGRAMMER:Scott Hensley c** c** FUNCTIONAL DESCRIPTION: This routine computes the curvature for c** of various types required for ellipsoidal or spherical earth c** calculations. c** c** ROUTINES CALLED:none c** c** NOTES: none c** c** UPDATE LOG: c** c***************************************************************** real*8 function reast(r_a,r_e2,r_lat) implicit none real*8 r_a,r_e2,r_lat reast = r_a/sqrt(1.d0 - r_e2*sin(r_lat)**2) end real*8 function rnorth(r_a,r_e2,r_lat) implicit none real*8 r_a,r_e2,r_lat rnorth = (r_a*(1.d0 - r_e2))/(1.d0 - r_e2*sin(r_lat)**2)**1.5d0 end real*8 function rdir(r_a,r_e2,r_hdg,r_lat) implicit none real*8 r_a,r_e2,r_lat,r_hdg,r_re,r_rn,reast,rnorth r_re = reast(r_a,r_e2,r_lat) r_rn = rnorth(r_a,r_e2,r_lat) rdir = (r_re*r_rn)/(r_re*cos(r_hdg)**2 + r_rn*sin(r_hdg)**2) end c23456789012345678901234567890123456789012345678901234567890123456789012