TOGEOD.M
资源名称:easy.zip [点击查看]
上传用户:sfyaiting
上传日期:2009-10-25
资源大小:320k
文件大小:2k
源码类别:
GPS编程
开发平台:
Matlab
- function [dphi,dlambda,h] = togeod(a,finv,X,Y,Z)
- %TOGEOD Subroutine to calculate geodetic coordinates
- % latitude, longitude, height given Cartesian
- % coordinates X,Y,Z, and reference ellipsoid
- % values semi-major axis (a) and the inverse
- % of flattening (finv).
- % The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc)
- % The output units of angular quantities will be in decimal degrees
- % (15.5 degrees not 15 deg 30 min). The output units of h will be the
- % same as the units of X,Y,Z,a.
- % Copyright (C) 1987 C. Goad, Columbus, Ohio
- % Reprinted with permission of author, 1996
- % Fortran code translated into MATLAB
- % Kai Borre 03-30-96
- h = 0;
- tolsq = 1.e-10;
- maxit = 10;
- % compute radians-to-degree factor
- rtd = 180/pi;
- % compute square of eccentricity
- if finv < 1.e-20
- esq = 0;
- else
- esq = (2-1/finv)/finv;
- end
- oneesq = 1-esq;
- % first guess
- % P is distance from spin axix
- P = sqrt(X^2+Y^2);
- % direct calculation of longitude
- if P > 1.e-20
- dlambda = atan2(Y,X)*rtd;
- else
- dlambda = 0;
- end;
- if (dlambda < 0)
- dlambda = dlambda + 360;
- end
- % r is distance from origin (0,0,0)
- r = sqrt(P^2+Z^2);
- if r > 1.e-20
- sinphi = Z/r;
- else
- sinphi = 0;
- end
- dphi = asin(sinphi);
- % initial value of height = distance from origin minus
- % approximate distance from origin to surface of ellipsoid
- if r < 1.e-20
- h = 0;
- break;
- end;
- h = r-a*(1-sinphi*sinphi/finv);
- % iterate
- for i = 1:maxit
- sinphi = sin(dphi);
- cosphi = cos(dphi);
- % compute radius of curvature in prime vertical direction
- N_phi = a/sqrt(1-esq*sinphi*sinphi);
- % compute residuals in P and Z
- dP = P - (N_phi + h) * cosphi;
- dZ = Z - (N_phi*oneesq + h) * sinphi;
- % update height and latitude
- h = h+(sinphi*dZ+cosphi*dP);
- dphi = dphi+(cosphi*dZ-sinphi*dP)/(N_phi + h);
- % test for convergence
- if (dP*dP + dZ*dZ < tolsq)
- break;
- end
- % Not Converged--Warn user
- if i == maxit
- fprintf([' Problem in TOGEOD, did not converge in %2.0f',...
- ' iterationsn'],i)
- end;
- end;
- dphi = dphi*rtd;
- %%%%%%%% end togeod.m %%%%%%%%%%%%%%%%%%%%%%