NavList:

   

Reply

/*

   NAVIGATION  (nav.c)  Version 1.1  Jan, 1993

   Calculates position (lat and lon) from several observations by least
   squares fitting to the altitude of any number of bodies vs. time.
   North latitude/declination is positive as is West longitude.

   The body/bodies can be anything.

   This program computes a position fix after inputing each sight.  The
   fix is displayed after each computation.  This lets you see the fix
   get better as more sights are added, but it does much more computing
   than necessary.  See the comments below if you wish to turn off this
   "feature".

   The GHA/dec/alt of the observations is input in decimal format.  Note
   that dead reckoning information can be included by substituting longitude
   for GHA, latitude for declination, and 90.0 for altitude.

   The algorithm follows Metcalf and Metcalf, "On the Overdetermined
Celestial
   Fix", NAVIGATION, Vol. 38, No. 1, Spring 1991, pp 79-89.

   Everything is done in double precision, since numerical roundoff is a
   potential problem in any iteration process.

   No error estimate is provided in this program.  The best error estimate
   would be the covariance matrix described in Severance, "Overdetermined
   Celestial Fix by Iteration", NAVIGATION, Vol. 36, No. 4, Winter 1989.

   No corrections are applied to the sights:  The altitude input must be Ho.

   No correction for the motion of the vessel has been included.  To include
   such a correction, see Metcalf, T. R., "Advancing Celestial Circles of
   Position", NAVIGATION, Vol. 38, No. 3, Fall 1991, pp 285-288.

   This program compiles and runs on a Sun SPARCstation 10 (Unix) and may or
   may not compile on any other machine.

   Compile:  cc -o nav nav.c -lm

   The program takes input from a file in the format

   GHA1  dec1  altitude1
   GHA2  dec2  altitude2
   GHA3  dec3  altitude3
   GHA4  dec4  altitude4
     .     .      .
     .     .      .
     .     .      .
    0.0   0.0    0.0


   where GHA = Greenwich Hour Angle of the body
         dec = declination of the body (North positive, South negative)
         altitude = observed altitude of the body (decimal degrees)

   Note: the last line in the input file must be all zeroes to termintate
the
         input.

   To run the program:    nav < data_file


   - Tom Metcalf   10/1991

   Version 1.1 changes from 1.0:

     - Includes weighting to make sights of equal weight.  See Metcalf, T.
R.,
       "An Extension to the Overdetermined Celestial Fix", NAVIGATION,
       Vol. 39, No. 4, Winter 1992-93, pp. 477-480.

Contact metcalf@uhifa.ifa.hawaii.edu for details of the algorithm (send a
surface mail address).

----------------------------------------------------------------------------
---

   This software is provided "as is" and is subject to change without
   notice.  No warranty of any kind is made with regard to this software,
   including, but not limited to, the implied warranties of
   merchantability and fitness for a particular purpose.  The author shall
   not be liable for any errors or for direct, indirect, special,
   incidental or consequential damages in connection with the furnishing,
   performance, or use of this software:  use it at your own risk.

   Copyright 1993 by Thomas R. Metcalf.  Permission is granted to any
   individual or institution to use, copy, or redistribute this software
   so long as it is not sold for profit and provided that this copyright
   notice and the above disclaimer are retained.

*/

#include 
#include 

#define DEG_TO_RAD 0.017453292519943295
#define PI 3.141592653589793238
#define LIMITING_ALTITUDE 89.9  /* Max Altitude for weighting correction */
                                /* cos^2(Hmax) = 10^(9.7-p)              */
                                /* p=12 digits, Hmax = 86   degrees      */
                                /* p=15 digits, Hmax = 89.9 degrees      */

#define MAXIT 100         /* Max number of iterations in Newton's method */

main(argc,argv)
int argc;
char *argv[];

{
  double gha,dec,alt;
  double sd,cd,sg,cg,sh,ch,nobs,weight;
  double G11,G12,G13,G22,G23,G33;
  double r[3],e1[3],e2[3],e3[3];
  double norme1,norme2,norme3;
  double alpha1,alpha2,alpha3,beta1,beta2,beta3;
  double a0,a1,Q,R,theta,sqrtQ;
  double bracket1,bracket4,bracket5;
  double mu,term1,term2,term3;
  double u,v,w,l,L;
  double n3;
  double hms,azim;
  double e,dofv,ap;
  double
u_bound,u_bound1,u_bound2,u_bound3,l_bound,l_bound1,l_bound2,l_bound3;
  double atan(),asin(),acos(),sin(),cos(),tan(),sqrt(),rtsafe(),fabs();
  int n,iterations;
  register loop;


    /* Initialize variables */
  n = 0;
  G11 = 0.0;
  G12 = 0.0;
  G13 = 0.0;
  G22 = 0.0;
  G23 = 0.0;
  G33 = 0.0;
  r[0] = 0.0;
  r[1] = 0.0;
  r[2] = 0.0;
  nobs = 0.0;

  if (argc>1) {  /* This allows an output title to be put on the command
line */
     printf("\n");
     for (loop=1; loop LIMITING_ALTITUDE*DEG_TO_RAD) weight=1.0;
               else weight=1.0/(ch*ch);

               G11 += weight*sd*sd;
        G12 += weight*sd*cd*cg;
        G13 += weight*sd*cd*sg;
               G22 += weight*cd*cd*cg*cg;
               G23 += weight*cd*cd*sg*cg;
        r[0] += weight*sd*sh;
        r[1] += weight*cd*cg*sh;
        r[2] += weight*cd*sg*sh;
               nobs += weight;

        ++n;
            }
         } while (n < 3); /* End of small loop which gets first 3 data
points */

/* MAIN LOOP should end here to compute the fix only once. */


  G33 = ((double) nobs) - G11 - G22;

  bracket1 = (G11*G22-G12*G12);   /* These are used more than once */
  bracket4 = (G12*G23-G13*G22);
  bracket5 = (G13*G12-G11*G23);

    /* Get values used in the computation of the eigenvalues */

  a0 = -bracket4*G13 - bracket5*G23 - (G11*G22-G12*G12)*G33;
  a1 = bracket1 + (G11*G33-G13*G13) + (G22*G33-G23*G23);

  n3 = (double) nobs/3.0;

  Q = n3*n3 - a1/3.0;
  sqrtQ = sqrt(Q);
  R = a0/2.0 + n3*(a1/6.0-Q);
  theta = acos(R/(Q*sqrtQ));

    /* Compute the eigenvalues of the G matrix */

  alpha1 = n3 - 2.0*sqrtQ*cos(theta/3.0);
  alpha3 = n3 - 2.0*sqrtQ*cos(theta/3.0 + 2.094395102);
  alpha2 = (double) nobs - alpha3 - alpha1;

/*
  printf("\nEigenvalues: %.3e %.3e %.3e",alpha1,alpha2,alpha3);
/*

    /* Compute the eigenvectors of the G matrix */

  edot(e1,bracket1,bracket4,bracket5,G11,G22,G13,G23,alpha1);
  edot(e2,bracket1,bracket4,bracket5,G11,G22,G13,G23,alpha2);
  edot(e3,bracket1,bracket4,bracket5,G11,G22,G13,G23,alpha3);

    /* Compute the beta values */

  bdot(&beta1,r,e1,&norme1);
  bdot(&beta2,r,e2,&norme2);
  bdot(&beta3,r,e3,&norme3);

/*
  printf("\nBetas: %.3e %.3e %.3e",beta1,beta2,beta3);
*/

    /* Check for ambiguous solutions */

  if ((fabs(alpha1)<1.0e-13 && fabs(beta1)<1.0e-13) ||
      (fabs(beta1)<1.0e-13 && fabs(beta2)<1.0e-13 && fabs(beta3)<1.0e-13)) {
     printf("\nERROR: Ambiguous Solution ... Need more data\n");
  }
  else {

    /* Do the mu iteration using Newton's method */

     u_bound1 = alpha1-fabs(beta1);    /* Get upper bound on mu */
     u_bound2 = alpha2-fabs(beta2);
     u_bound3 = alpha3-fabs(beta3);
     u_bound = u_bound1;
     if (u_bound2 < u_bound) u_bound=u_bound2;
     if (u_bound3 < u_bound) u_bound=u_bound3;

     l_bound1 = alpha1-1.73205080757*fabs(beta1);  /* get lower bound on mu
*/
     l_bound2 = alpha2-1.73205080757*fabs(beta2);
     l_bound3 = alpha3-1.73205080757*fabs(beta3);
     l_bound = l_bound1;
     if (l_bound2 < l_bound) l_bound = l_bound2;
     if (l_bound3 < l_bound) l_bound = l_bound3;

     /* rtsafe is Newton's method, accounting for the known upper and lower
     bounds on mu.  This is slightly slower than the regular Newton's
method,
     but the extra security is worth it. See "Numerical Recipes in C" */

     mu = rtsafe(l_bound,u_bound,(double)1.0e-7,
                 alpha1,alpha2,alpha3,beta1,beta2,beta3,&iterations);

  /* Check for proper convergence.  In theory, the iteration should
     never fail, but ... */

     if (iterations>MAXIT || mu>alpha1) {
       fprintf(stderr,"\n\nWARNING: Convergence error has occurred!  ");
       fprintf(stderr,"Don't trust fix ...\n\n");
     }

     term1 = beta1/(alpha1-mu);   /* These are used more than once */
     term2 = beta2/(alpha2-mu);
     term3 = beta3/(alpha3-mu);

    /* Compute u, v, w */

     u = term1*e1[0]/norme1 + term2*e2[0]/norme2 + term3*e3[0]/norme3;
     v = term1*e1[1]/norme1 + term2*e2[1]/norme2 + term3*e3[1]/norme3;
     w = term1*e1[2]/norme1 + term2*e2[2]/norme2 + term3*e3[2]/norme3;

    /* Compute the position fix */

     if (fabs(u) > 1.0) u = u/fabs(u);
     L = asin(u);
     l = atan((w/v));

     if (v<0.0) {      /* Resolve ambiguity in the principal value of atan()
*/
       if (w>0.0) {
      l += 3.14159265359;
    }
    else {
      l -= 3.14159265359;
    }
     }

     L = L / DEG_TO_RAD;   /* Convert to degrees from radians */
     l = l / DEG_TO_RAD;

     if (gha!=0.0 || dec!=0.0 || alt!=0.0) {
        /* print the computed position thus far */
        printf("\n%8.4f %8.4f %8.4f %3d %8.4f
%9.4f",gha/DEG_TO_RAD,dec/DEG_TO_RAD,alt/DEG_TO_RAD,iterations,L,l);
     }

   }

  } while (gha!=0.0 || dec!=0.0 || alt!=0.0);  /* End of MAIN LOOP */

  /* Move the above while statement up to the commented position to
     turn off the multiple fix computations. */

  printf("\n\n");
  printf("\nPosition: Latitude = %8.4f, Longitude = %9.4f",L,l);
  printf("\n\n");

}  /* end of main() */



int edot(e,bracket1,bracket4,bracket5,G11,G22,G13,G23,alpha)
double *e,bracket1,bracket4,bracket5,G11,G22,G13,G23,alpha;
{

/* Computes the eigenvectors of the G matrix */

   e[0] = bracket4 + G13*alpha;
   e[1] = bracket5 + G23*alpha;
   e[2] = bracket1 - (G11+G22)*alpha + alpha*alpha;

}



int bdot(beta,r,e,norme)
double *beta,*r,*e,*norme;

/* Computes the beta values for the mu iteration */

{

  double sqrt();
  register loop;

  *norme = sqrt(e[0]*e[0] + e[1]*e[1] + e[2]*e[2]);

  *beta = 0.000;

  for (loop=0; loop<=2; ++loop) {
     *beta += r[loop]*e[loop];
  }
  *beta = *beta/(*norme);

}


double rtsafe(x1,x2,xacc,alpha1,alpha2,alpha3,beta1,beta2,beta3,iter)
double x1,x2,xacc,alpha1,alpha2,alpha3,beta1,beta2,beta3;
int *iter;

/* Safe Newton's iteration adapted from "Numerical Recipes in C" by
   Press et. al.
*/

{
 int j;
 double df,dx,dxold,f,fh,fl;
 double swap,temp,xh,xl,rts;
        void newtfunc();
        double fabs();

 newtfunc(x1,&fl,&df,alpha1,alpha2,alpha3,beta1,beta2,beta3);
 newtfunc(x2,&fh,&df,alpha1,alpha2,alpha3,beta1,beta2,beta3);
 if (fl*fh > 0.0)
           printf("\nRoot must be bracketed in RTSAFE %f %f\n",fl,fh);
        if (fh == 0.0) x2 = x2 + 0.1;
        if (fl == 0.0) x1 = x1 - 0.1;
 if (fl < 0.0) {
  xl=x1;
  xh=x2;
 } else {
  xh=x1;
  xl=x2;
  swap=fl;
  fl=fh;
  fh=swap;
 }
        rts=0.0;   /* Initial guess is 0.0 */
 dxold=fabs(x2-x1);
 dx=dxold;
 newtfunc(rts,&f,&df,alpha1,alpha2,alpha3,beta1,beta2,beta3);
 for (j=1;j<=MAXIT;j++) {
  if ((((rts-xh)*df-f)*((rts-xl)*df-f) >= 0.0)
   || (fabs(2.0*f) > fabs(dxold*df))) {
   dxold=dx;
   dx=0.5*(xh-xl);
   rts=xl+dx;
                        *iter=j;
   if (xl == rts) return rts;
  } else {
   dxold=dx;
   dx=f/df;
   temp=rts;
   rts -= dx;
                        *iter=j;
   if (temp == rts) return rts;
  }
                *iter=j;
  if (fabs(dx) < xacc) return rts;
  newtfunc(rts,&f,&df,alpha1,alpha2,alpha3,beta1,beta2,beta3);
  if (f < 0.0) {
   xl=rts;
   fl=f;
  } else {
   xh=rts;
   fh=f;
  }
 }
 printf("\nMaximum number of iterations exceeded in RTSAFE\n");
        return((double)0.0);
}


void newtfunc(mu,g,gp,alpha1,alpha2,alpha3,beta1,beta2,beta3)
double mu,*g,*gp,alpha1,alpha2,alpha3,beta1,beta2,beta3;

/* The g(mu) function to be solved by Newton's method. Returns both the
   function and its derivative (gp). */

{
     double term1,term2,term3;

     term1 = beta1/(alpha1-mu);
     term2 = beta2/(alpha2-mu);
     term3 = beta3/(alpha3-mu);

     *g = term1*term1 + term2*term2 + term3*term3 - 1.0;
     *gp = (2.0/(alpha1-mu))*term1*term1 +
     (2.0/(alpha2-mu))*term2*term2 +
    (2.0/(alpha3-mu))*term3*term3;

}

   

Reply