1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-10-30 14:46:23 +00:00
gnss-sdr/utils/matlab/libs/geoFunctions/togeod.m

111 lines
2.7 KiB
Mathematica
Raw Normal View History

2018-03-30 10:04:14 +00:00
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).
%
% [dphi, dlambda, h] = togeod(a, finv, X, Y, Z);
%
% 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.
%
% Inputs:
% a - semi-major axis of the reference ellipsoid
% finv - inverse of flattening of the reference ellipsoid
% X,Y,Z - Cartesian coordinates
%
% Outputs:
% dphi - latitude
% dlambda - longitude
% h - height above reference ellipsoid
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: 1987 C. Goad, 1996 Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
2018-03-30 10:04:14 +00:00
%==========================================================================
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 axis
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;
return
end
h = r - a*(1-sinphi*sinphi/finv);
% iterate
for i = 1:maxit
sinphi = sin(dphi);
cosphi = cos(dphi);
2018-03-30 10:04:14 +00:00
% compute radius of curvature in prime vertical direction
N_phi = a/sqrt(1-esq*sinphi*sinphi);
2018-03-30 10:04:14 +00:00
% compute residuals in P and Z
dP = P - (N_phi + h) * cosphi;
dZ = Z - (N_phi*oneesq + h) * sinphi;
2018-03-30 10:04:14 +00:00
% update height and latitude
h = h + (sinphi*dZ + cosphi*dP);
dphi = dphi + (cosphi*dZ - sinphi*dP)/(N_phi + h);
2018-03-30 10:04:14 +00:00
% test for convergence
if (dP*dP + dZ*dZ < tolsq)
break;
end
2018-03-30 10:04:14 +00:00
% Not Converged--Warn user
if i == maxit
fprintf([' Problem in TOGEOD, did not converge in %2.0f',...
' iterations\n'], i);
end
end % for i = 1:maxit
dphi = dphi * rtd;
%%%%%%%% end togeod.m %%%%%%%%%%%%%%%%%%%%%%