ocorient.f

Go to the documentation of this file.

        subroutine ocorient(pos,vel,att,rm,coef)
 
c  this subroutine performs a simple calculation of the sensor
c  orientation from the orbit position vector and input values of the
c  attitude offset angles.  the calculations assume that the angles
c  represent the roll, pitch and yaw offsets between the local vertical
c  reference frame(at the spacecraft position) and the sensor frame. 
c  sensor tilt angles are assumed to be included in the pitch angle. 
c  the outputs are the matrix which represents the transformation from
c  the geocentric rotating to sensor frame, and the coefficients which
c  represent the earth scan track in the sensor frame.
 
c  the reference ellipsoid uses an equatorial radius of 6378.137 km and
c  a flattening factor of 1/298.257 (wgs 1984). 
 
 
c  calling arguments
 
c  name         Type    i/o     description
c
c  pos(3)       r*4      i      orbit position vector ecef(km)
c  vel(3)       r*4      i      orbit velocity vector ecef(km/sec)
c  att(3)       r*4      i      attitude offsets(euler angles in 
c                                degrees); referenced to local vertical 
c                                coordinates; order is roll, pitch, yaw 
c(x, y, z)
c  rm(3,3)      r*4      o      sensor orientation matrix
c  coef(10)     r*4      o      scan path coefficients
c
c       subprograms called(attached):
 
c       crossp          compute cross product of two vectors
c       euler           compute matrix from euler angles
c       matmpy          multiply two 3x3 matrices
c
c       Program written by:     frederick s. patt
c                               general sciences corporation
c                               july 22, 1992
c
c       modification history:
c
c       added improved calculation of local vertical reference frame and
c        modified calling argument names.  
c        f. s. patt, september 30, 1992
c
c       expanded vector normalization in-line to eliminate subroutine 
c        call.  f. s. patt, october 19, 1992
c
c       eliminated redundant calculations, changed to correspond to
c       paper, "Exact closed-form geolocation algorithm for earth
c       survey sensors", international journal of remote sensing, patt
c       and gregg, 1993.  w. gregg, 4/5/93.
c
c       modified to support three-dimensional view vectors by computing
c       coefficients array with all 10 ellipsoid terms.  
c       f. s. patt, november 25, 1996
c
      real*8 pi,radeg,re,rem,f,omf2,omegae
      real vc(3),xpri(3),ypri(3),zpri(3),yrp(3,3),rn(3,3),rm(3,3)
      real pos(3),vel(3),att(3),coef(10)
      common /gconst/pi,radeg,re,rem,f,omf2,omegae
c
c  compute correction to orbit velocity vector in earth-centered
c   earth-fixed(ecef) frame; this involves subtracting effect of earth
c   rotation rate on velocity to get correct scan plane orientation in
c   ecef frame.
      vc(1) = vel(1) - omegae*pos(2)
      vc(2) = vel(2) + omegae*pos(1)
      vc(3) = vel(3)
 
c  determine nadir frame reference axes
c  uses method of local ellipsoid approximation good to 0.3 arcsecond
c   compute z axis as local nadir vector
        pm = dsqrt(dble(pos(1)*pos(1)+pos(2)*pos(2)+pos(3)*pos(3)))
        omf2p = (omf2*rem + pm - rem)/pm
        pxy = pos(1)*pos(1)+pos(2)*pos(2)
        temp = dsqrt(dble(pos(3)*pos(3) + omf2p*omf2p*pxy))
        zpri(1) = -omf2p*pos(1)/temp
        zpri(2) = -omf2p*pos(2)/temp
        zpri(3) = -pos(3)/temp
c   compute y axis along negative orbit normal
        call crossp(vc,zpri,ypri)
        yprim = dsqrt(dble(ypri(1)*ypri(1) + ypri(2)*ypri(2) +
     +                     ypri(3)*ypri(3)))
        ypri(1) = -ypri(1)/yprim
        ypri(2) = -ypri(2)/yprim
        ypri(3) = -ypri(3)/yprim
c   compute x axis to complete orthonormal triad
        call crossp(ypri,zpri,xpri)
c  store in matrix 
        do i=1,3
            rn(1,i)=xpri(i)
            rn(2,i)=ypri(i)
            rn(3,i)=zpri(i)
        end do
c
c  convert attitude(euler) angles to yrp matrix
        call oceuler(att,yrp)
 
c  compute sensor orientation matrix 
        call matmpy(yrp,rn,rm)
 
c  compute coefficients of intersection ellipse in scan plane
        rd=1.d0/omf2
        coef(1) = 1.d0+(rd-1.d0)*rm(1,3)*rm(1,3)
        coef(2) = 1.d0+(rd-1.d0)*rm(2,3)*rm(2,3)
        coef(3) = 1.d0+(rd-1.d0)*rm(3,3)*rm(3,3)
        coef(4) = (rd-1.d0)*rm(1,3)*rm(2,3)*2.d0
        coef(5) = (rd-1.d0)*rm(1,3)*rm(3,3)*2.d0
        coef(6) = (rd-1.d0)*rm(2,3)*rm(3,3)*2.d0
        coef(7) = (rm(1,1)*pos(1)+rm(1,2)*pos(2)+rm(1,3)*
     *            pos(3)*rd)*2.d0
        coef(8) = (rm(2,1)*pos(1)+rm(2,2)*pos(2)+rm(2,3)*
     *            pos(3)*rd)*2.d0
        coef(9) = (rm(3,1)*pos(1)+rm(3,2)*pos(2)+rm(3,3)*
     *            pos(3)*rd)*2.d0
        coef(10) = pos(1)*pos(1)+pos(2)*pos(2)+pos(3)*pos(3)*rd-re*re
c
        return
        end
 
c       subroutine crossp(v1,v2,v3)
c  originally here but removed as it is a independent routine in library
 
        subroutine oceuler(a,xm)
c  computes coordinate transformation matrix corresponding to euler 
c  sequence.  the order of angles in the input array is yaw, roll, 
c  pitch; according to osc, the order of the rotations is the reverse
c  of this; the roll and pitch angles are about the negative y and z
c  axes, respectively, while the yaw angle is about the positive x axis. 
 
c  reference:  wertz, appendix e; osc, personal communication 
 
c  calling arguments
 
c  name         Type    i/o     description
c
c  a(3)         r*4      i      input array of euler angles(degrees)
c  xm(3,3)      r*4      o      output transformation matrix
c
c  frederick s. patt, gsc, sometime in 1992.
c
c  modification history:
c
c
c  modified to change order of rotations to pitch, roll, yaw(-z, -y, -x)
c  f. s. patt, gsc, september 29, 1996.
c
c  removed negative signs on y and z rotations for octs; order of rotations
c  is yaw, pitch, roll(z, y, x)
 
        real xm1(3,3),xm2(3,3),xm3(3,3),xm(3,3),xmm(3,3),a(3)
        real*8 radeg
        radeg = 180.d0/3.14159265359d0
 
c  initialize all matrix elements to zero.      
        do i=1,3
          do j=1,3
            xm1(i,j) = 0.d0
            xm2(i,j) = 0.d0
            xm3(i,j) = 0.d0
          end do
        end do
 
c  compute sines and cosines
        c1=dcos(a(1)/radeg)
        s1=dsin(a(1)/radeg)
        c2=dcos(a(2)/radeg)
        s2=dsin(a(2)/radeg)
        c3=dcos(a(3)/radeg)
        s3=dsin(a(3)/radeg)
 
c  convert individual rotations to matrices
        xm1(1,1)=1.d0
        xm1(2,2)=c1
        xm1(3,3)=c1
        xm1(2,3)=s1
        xm1(3,2)=-s1
        xm2(2,2)=1.d0
        xm2(1,1)=c2
        xm2(3,3)=c2
        xm2(3,1)=s2
        xm2(1,3)=-s2
        xm3(3,3)=1.d0
        xm3(2,2)=c3
        xm3(1,1)=c3
        xm3(1,2)=s3
        xm3(2,1)=-s3
 
c  compute total rotation as xm1*xm2*xm3 
        call matmpy(xm2,xm3,xmm)
        call matmpy(xm1,xmm,xm)
        return
        end
 
        subroutine matmpy(xm1,xm2,xm3)
c  computes matrix product of 3x3 matrices xm1 and xm2
 
c  calling arguments
 
c  name         Type    i/o     description
c
c  xm1(3,3)     r*4      i      input matrix
c  xm2(3,3)     r*4      i      input matrix
c  xm3(3,3)     r*4      o      output matrix
 
        real xm1(3,3),xm2(3,3),xm3(3,3)
        do i=1,3
            do j=1,3
                xm3(i,j) = 0.d0
                do k=1,3
                    xm3(i,j) = xm3(i,j) + xm1(i,k)*xm2(k,j)
                end do
            end do
        end do
        return
        end