attdet.f

Go to the documentation of this file.

        subroutine attdet( navblk, navctl, nad_bod, attxfm, 
     1     pos, vel, widphse1, widphfl1, widphse2, widphfl2, 
     1     sun_bod, sunbodfl, att_prev, delta_t, covar)
c
c  attdet( navblk, navctl, nad_bod, attxfm, 
c       pos, vel, widphse1, widphfl1, widphse2, 
c       widphfl2, sun_bod, sunbodfl, att_prev, delta_t, covar )
c
c  purpose: perform the actual attitude determination for one line
c
c  calling arguments:
c
c  name         Type    i/o     description
c  --------     ----    ---     -----------
c  navblk       struct  i/o     navigation block including:
c                               in: sun_ref - sun ref in geo coord sys
c                                   lcl_vert - local vertical
c                               out: sen_frame - attitude matrix
c                                    att_ang - attitude angles in the
c                                       order yaw, roll, pitch
c  navctl       struct   i      navigation controls. used are:
c                               redoyaw - # iterations to redo yaw 
c                               computation
c                               yawtol - difference between yaw guess
c                               and computed yaw 
c  nad_bod      r*4      i      size 3 unit nadir vector in spacecraft frame
c  attxfm       r*4      i      size 3 by 3 attitude transform matrix - 
c                               the transform from geocentric to 
c                               orbit coordinates
c  pos          r*4      i      size 3 orbit position vector(km)
c  vel          r*4      i      size 3 orbit velocity vector(km/sec)
c  widphse1     r*4      i      size 2 processed earth width 
c                               and phase for sensor 1
c  widphfl1     i*4      i      flag for earth sensor 1 - 0 = good
c  widphse2     r*4      i      size 2 processed earth width 
c                               and phase for sensor 2
c  widphfl2     i*4      i      flag for earth sensor 2 - 0 = good
c  sun_bod      r*4      i      size 3 weighted sun vectors in spacecraft 
c                               frame
c  sunbodfl     i*4      i      flag for sun sensor data - 0 = good
c  att_prev     r*4      i      size 3 attitude angles from previous scan line
c  delta_t      r*4      i      time difference from previous scan line
c(set to 0 if first line in frame, negative if
c                               propagating backwards in time)
c  covar        r*4      i      size 3 array of covariance matrix diagonal 
c                               elements
c
c
c  by: w. robinson, gsc, 14 apr 93
c
c  notes: the initial yaw used to create the nadir vector from the
c       earth sensors is assumed to be the previous value.  If the 
c       resultant yaw computed here id different from the guess by 
c       the value yawtol, and the redoyaw is set, the nadir vector 
c       will be re-computed by calling routine earth again.  this will
c       be repeated up to redoyaw number of times 
c
c  modification history:
c
c  modified extraction of euler angles from attitude matrix to reflect
c  actual order and sign of rotations implemented by osc in the acs.
c  the order is 2-1-3 in the acs(not the spacecraft) frame.  the acs
c  axes defined as:  x toward the velocity(spacecraft -y axis); 
c  y toward the negative orbit normal(spacecraft -z axis); z toward 
c  the nadir(spacecraft x axis).  this is equivalent to a 
c  pitch-roll-yaw transformation, with the pitch and roll angles being 
c  negative about the spacecraft z and y axes.
c  f. s. patt, gsc, october 4, 1996. 
c
c  modified the order of operations to first transform the matrix constructed
c  from the reference vectors to the orbital frame, then use that matrix to
c  compute the orbit-to-spacecraft transformation.  this will give the same
c  results as before, but allows the attitude matrix to be a small rotation.
c  f. s. patt, saic gsc, february 3, 1998. 
c
c  modified attitude angle calculation to weight the yaw angle according to
c  the sun/nadir angle to prevent blowing up at subsolar point.
c  f. s. patt, saic gsc, march 9, 1998.
c  
c  rewrote this routine to implement attitude determination using a kalman
c  filter instead of the deterministic algorithm used previously.  this was
c  done to resolve multiple issues:  reduce jitter at sensor transitions,
c  improve yaw accuracy at the subsolar point and handle sensor gaps.  the
c  initial version uses a simple dynamics model which assumes an inertial
c  spin axis(not necessarily parallel to the orbit normal).
c  all of the equations are taken from wertz, section 13.5.2, pp. 462-465.
c  f. s. patt, saic gsc, june 1, 1998.
c
c  reduced the state noise value(from 1.e-8 to 0.25e-8) to reduce the yaw
c  variation in the middle of the orbit.
c  f. s. patt, saic gsc, june 22, 1998.
c
c  corrected a(minor) error in the calculation of the partial derivatives.
c  f. s. patt, saic gsc, october 19, 1998.
c 
c  reduced the state noise value for roll and yaw to 6.e-10, to constrain the 
c  yaw angle near the subsolar point.  f. s. patt, saic gsc, november 25, 1998.
c
c  added data initialization statements for p and q to ensure they get saved 
c  between calls.   f. s. patt, saic gsc, april 14, 1999
c
c  modifed calculation of sun vector measurement covariance to avoid 
c  numerical round-off errors for sun vector very close to -x axis.
c  f. s. patt, saic gsc, august 27, 1999
c
c  increased state noise for roll and yaw to 2.5e-9 based on results of testing
c  with revised horizon scanner calibration and sun sensor alignments.
c  f. s. patt, saic gsc, september 20, 1999
c
c  modified state noise calculation to use delta_t**2 (instead of abs(delta_t))
c  to reduce noise for hrpt/lac, and changed qvar(from 2.5e-9 to 4.0e-9) to 
c  offset this for gac; this is based on initial results of testing for the 
c  mergered 1-km(mlac) files.
c  f. s. patt, saic, november 22, 2002
 
      implicit none
#include "nav_cnst.fin"
#include "navblk_s.fin"
#include "navctl_s.fin"
c
      type(navblk_struct) :: navblk   ! note we only want the block for 1 line
      type(navctl_struct) :: navctl
      integer*4 widphfl1, widphfl2, sunbodfl
      real*4 nad_bod(3), sun_bod(3), attxfm(3,3), widphse1(2), 
     1     widphse2(2), pos(3), vel(3), att_prev(3), delta_t, covar(3)
c
      real*8 xk(3,6), p(3,3), r(6,6), g(6,3), xid(3,3), d(3,3), q(3,3)
      real*8 t33(3,3), t66(6,6), t36(3,6), yg(6), gt(3,6), dt(3,3) 
      real*8 t6(6), t3(3), r33(3,3), s33(3,3), xx(6,3)
      real*4 dmda(3,3,3), dvda(3,3), cpit, spit, crol, srol, cyaw, syaw
      real*4 att_ap(3), apm(3,3), sunvar, earvar, qvar(3), vmag, tv(3)
      integer*4 nyaw, ier, i, j, nadbodfl
      real*4 pmag, yawest, sr(3), nr(3)
      logical recomp, update
 
c   set variances for sun and earth vectors and state noise
      data sunvar/1.e-6/, earvar/3.e-6/, qvar/2*4.0e-9,4.0e-9/
c   identity matrix
      data xid/1.d0,3*0.d0,1.d0,3*0.d0,1.d0/
      data d/1.d0,3*0.d0,1.d0,3*0.d0,1.d0/
      data p/1.d0,3*0.d0,1.d0,3*0.d0,1.d0/
      data q/9*0.d0/
      data r/36*0.d0/
    
c   compute state propagation and noise covariance matrices
      d(2,1) = delta_t*.00106d0
      d(1,2) = -d(2,1)
      do i=1,3
c         q(i,i) = abs(delta_t)*qvar(i)
         q(i,i) = delta_t*delta_t*qvar(i)
      end do
 
c   If first scan of scene(delta_t = 0), set state covariance and attitude
      if (delta_t.eq.0.0) then
         do i=1,3
            att_ap(i) = 0.0
            do j=1,3
               p(i,j) = xid(i,j)
               apm(i,j) = xid(i,j)
            end do
         enddo
      else
 
c   compute a priori attitude and matrix based on previous attitude
 
c      propagate attitude using simple dynamics model(wertz, eq. 13-86)
c      may replace this with a more sophisticated model at a later date
c      need to do this in-line to handle mixed precision
         do i=1,3
            att_ap(i) = d(i,1)*att_prev(1) + d(i,2)*att_prev(2)
     *           + d(i,3)*att_prev(3)
         end do
         call euler(att_ap, apm)
 
c      propagate state covariance matrix and add state noise(wertz, eq. 13-88)
         call dmatmp(d,p,t33,3,3,3)
         call dxpose(d,dt,3,3)
         call dmatmp(t33,dt,p,3,3,3)
         do i=1,3
            do j=1,3
               p(i,j) = p(i,j) + q(i,j)
            end do
         end do
      end if
      
c      call matmpy(apm, attxfm, m)
 
c   compute reference vectors in a prior spacecraft frame
      pmag = vmag(pos)
c      call matvec(m, pos, nr)
c      do i=1,3 
c         nr(i) = -nr(i)/pmag
c      end do
c      call matvec(m, navblk%sun_ref(1), sr)
 
c   compute partial derivatives of attitude matrix with respect to angles
      cyaw = cos(att_ap(1)/radeg)
      syaw = sin(att_ap(1)/radeg)
      crol = cos(att_ap(2)/radeg)
      srol = sin(att_ap(2)/radeg)
      cpit = cos(att_ap(3)/radeg)
      spit = sin(att_ap(3)/radeg)
      do i=1,3
         dmda(1,i,1) = 0.0
         dmda(2,i,1) = apm(3,i)
         dmda(3,i,1) = -apm(2,i)
         dmda(i,1,2) = -apm(i,3)*cpit
         dmda(i,2,2) = apm(i,3)*spit
         dmda(i,1,3) = apm(i,2)
         dmda(i,2,3) = -apm(i,1)
         dmda(i,3,3) = 0.0
      end do
      dmda(1,3,2) = crol
      dmda(2,3,2) = -syaw*srol
      dmda(3,3,2) = -cyaw*srol
 
c   initialize for yaw iteration if necessary
 
      nyaw = 0
      recomp = .true.
      update = .false.
      yawest = att_ap(1)
c
      dowhile( recomp )
         call earth( pos, vel, widphse1, widphfl1, widphse2, widphfl2,
     1        yawest, navctl, nad_bod, nadbodfl, navblk%sun_ref(3) )
 
c          initialize arrays
         do i=1,6
            yg(i) = 0.d0
            r(i,i) = 1.d0
            do j=1,3
               g(i,j) = 0.d0
               xk(j,i) = 0.d0
            end do
         end do   
 
c          compute partial derivative matrix and difference vector
 
c          If there is valid sun data
         if (sunbodfl.eq.0) then
 
c       compute partials of sun vector with respect to attitude angles 
            call matvec(attxfm, navblk%sun_ref(1), tv)
 
            do i=1,3
               call matvec(dmda(1,1,i),tv,dvda(1,i))
            end do
            call matvec(apm,tv,sr)
 
c          load into g matrix and compute difference vector
            do i=1,3
               yg(i) = sun_bod(i) - sr(i)
               r(i,i) = sunvar*(1.0001d0 - sr(i)*sr(i))
c               r(i,i) = sunvar
               do j=1,3
                  g(i,j) = dvda(i,j)
               end do  
            end do   
         end if
 
c          If there is valid earth data
         if (nadbodfl.eq.0) then
 
c          compute partials of earth vector with respect to attitude angles 
            call matvec(attxfm, pos,tv)
            do i=1,3 
               tv(i) = -tv(i)/pmag
            end do
            do i=1,3
               call matvec(dmda(1,1,i),tv,dvda(1,i))
            end do
            call matvec(apm,tv,nr)
 
c          load into g matrix and compute difference vector
            do i=1,3
               yg(i+3) = nad_bod(i) - nr(i)
               r(i+3,i+3) = earvar
               do j=1,3
                  g(i+3,j) = dvda(i,j)
               end do  
            end do               
         end if
 
c      If there are observations, correct attitude and update covariance
         if ( (sunbodfl.eq.0) .or. (nadbodfl.eq.0) ) then
 
c          compute gain matrix(wertz, eq. 13-74)
            call dxpose(g,gt,6,3)
            call dmatmp(p,gt,t36,3,3,6)
            call dmatmp(g,t36,t66,6,3,6)
            do i=1,6
               t66(i,i) = t66(i,i) + r(i,i)
            end do
            call invert(t66,yg,6,6,t6,ier)
            call dmatmp(gt,t66,t36,3,6,6)
            call dmatmp(p,t36,xk,3,3,6)
 
c          compute update to attitude angles
            call dmatmp(xk,yg,t3,3,6,1)
            do i=1,3
               navblk%att_ang(i) = att_ap(i) + radeg*t3(i)
            end do
            update = .true.
 
c      Else, use propagated attitude
         else
            do i=1,3
               navblk%att_ang(i) = att_ap(i)
            end do
 
         end if
c
c          if the re-compute yaw value is greater than the 
c          the tolerence and 
c
         nyaw = nyaw + 1
         if( navctl%redoyaw .ge. nyaw .and. 
     1      abs( navblk%att_ang(1) - yawest) .gt. navctl%yawtol ) then
            recomp = .true.
            yawest = navblk%att_ang(1)
         else
            recomp = .false.
         end if
      end do
      
c       update covariance matrix(wertz, eq. 13-76)
      call dmatmp(xk,g,t33,3,6,3)
      do i=1,3
         do j=1,3
            t33(i,j) = xid(i,j) - t33(i,j)
         end do
      end do
      call dmatmp(t33,p,r33,3,3,3)
      call dxpose(t33,s33,3,3)
      call dmatmp(r33,s33,p,3,3,3)
      call dmatmp(xk,r,t36,3,6,6)
      call dxpose(xk,xx,3,6)
      call dmatmp(t36,xx,t33,3,6,3)
      do i=1,3
         do j=1,3
            p(i,j) = p(i,j) + t33(i,j)
         end do
      end do
 
      if (navctl%lvdbug.gt.2) then
         write (66,*) ((att_ap(i) - att_prev(i)),i=1,3)
         write (66,*) ((navblk%att_ang(i) - att_ap(i)),i=1,3)
         write (66,*) yg
         do i=1,6
            t6(i) = 0.d0
            do j=1,3
               t6(i) = t6(i) + g(i,j)*t3(j)
            end do
         end do
         write (66,*) t6
      end if
 
c      add diagonal elements of covariance matrix to output array
      do i=1,3
         covar(i) = p(i,i)
      end do
 
c
c       and end
c
      return
      end
 
        subroutine dmatmp(dm1, dm2, dm3, l, m, n)
c
c  dmatmp( dm1, dm2, dm3, l, m, n)
c
c  purpose: multiply two general double-precision matrices
c
c  calling arguments:
c
c     name         Type    i/o     description
c     --------     ----    ---     -----------
c     dm1          r*8      i       input matrix of size l-by-m
c     dm2          r*8      i       input matrix of size m-by-n
c     dm3          r*8      o       output matrix of size l-by-n
c     l            i*4      i       number of rows in dm1 and dm3
c     m            i*4      i       number of columns in dm1 and rows in dm2
c     n            i*4      i       number of columns in dm2 and dm3
c
c  by: f. s. patt, saic gsc, june 2, 1998
c
c
c  modification history:
c
      implicit none
      integer*4 l,m,n
      real*8 dm1(l,m),dm2(m,n),dm3(l,n)
      integer*4 i,j,k
 
c  compute dm3 as dm1 x dm2
      do i=1,l
         do j=1,n
            dm3(i,j) = 0.d0
            do k=1,m
               dm3(i,j) = dm3(i,j) + dm1(i,k)*dm2(k,j)
            end do
         end do
      end do
 
      return
      end
 
        subroutine dxpose(din, dout, n, m)
c
c  dxpose( din, dout, n, m )
c
c  purpose: form the transpose of matrix din to matrix dout
c
c  calling arguments:
c
c     name         Type    i/o     description
c     --------     ----    ---     -----------
c     din          r*8      i      size n by m input matrix
c     dout         r*8      o      size m by n output matrix
c     n            i*4      i      number of rows in din
c     m            i*4      i      number of columns in din 
c
c  by: f. s. patt, saic gsc, june 2, 1998
c
c  notes:  
c
c  modification history:
c
      implicit none
      integer*4 n, m
      real*8 din(n,m), dout(m,n)
c
      integer*4 i, j
c
c
      do i = 1, n
        do j = 1, m
          dout(j,i) = din(i,j)
        end do
      end do
c
      return
      end