Welcome to the NavList Message Boards.

NavList:

A Community Devoted to the Preservation and Practice of Celestial Navigation and Other Methods of Traditional Wayfinding

Compose Your Message

Message:αβγ
Message:abc
Add Images & Files
    or...
       
    Reply
    Nav.c
    From: Dan Allen
    Date: 2001 Apr 20, 8:03 PM

    /*
    
       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{at}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
    Browse Files

    Drop Files

    NavList

    What is NavList?

    Join NavList

    Name:
    (please, no nicknames or handles)
    Email:
    Do you want to receive all group messages by email?
    Yes No

    You can also join by posting. Your first on-topic post automatically makes you a member.

    Posting Code

    Enter the email address associated with your NavList messages. Your posting code will be emailed to you immediately.
    Email:

    Email Settings

    Posting Code:

    Custom Index

    Subject:
    Author:
    Start date: (yyyymm dd)
    End date: (yyyymm dd)

    Visit this site
    Visit this site
    Visit this site
    Visit this site
    Visit this site
    Visit this site