#include #include /* 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. */ #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; }