Cartesian to Geodetic
For many years, I have used the closed-form solution from Heikkinen [1] for converting Cartesian coordinates to geodetic latitude and altitude. I've never actually seen the original reference (which is in German). I coded up the algorithm from the table given in [2], and have used it ever since, in school and also at work. At one point I compared it with various other closed-form algorithms, and never found one that was faster and still had the same level of accuracy. However, I recently happened to stumble upon Olson's method [3], which seems to be better.
A Fortran implementation of Olson's algorithm is:
pure subroutine olson(rvec, a, b, h, lon, lat)
use, intrinsic :: iso_fortran_env, wp=>real64 !double precision
implicit none
real(wp),dimension(3),intent(in) :: rvec !position vector [km]
real(wp),intent(in) :: a ! geoid semimajor axis [km]
real(wp),intent(in) :: b ! geoid semiminor axis [km]
real(wp),intent(out) :: h ! geodetic altitude [km]
real(wp),intent(out) :: lon ! longitude [rad]
real(wp),intent(out) :: lat ! geodetic latitude [rad]
real(wp) :: f,x,y,z,e2,a1,a2,a3,a4,a5,a6,w,zp,&
w2,r2,r,s2,c2,u,v,s,ss,c,g,rg,rf,m,p,z2
x = rvec(1)
y = rvec(2)
z = rvec(3)
f = (a-b)/a
e2 = f * (2.0_wp - f)
a1 = a * e2
a2 = a1 * a1
a3 = a1 * e2 / 2.0_wp
a4 = 2.5_wp * a2
a5 = a1 + a3
a6 = 1.0_wp - e2
zp = abs(z)
w2 = x*x + y*y
w = sqrt(w2)
z2 = z * z
r2 = z2 + w2
r = sqrt(r2)
if (r < 100.0_wp) then
lat = 0.0_wp
lon = 0.0_wp
h = -1.0e7_wp
else
s2 = z2 / r2
c2 = w2 / r2
u = a2 / r
v = a3 - a4 / r
if (c2 > 0.3_wp) then
s = (zp / r) * (1.0_wp + c2 * (a1 + u + s2 * v) / r)
lat = asin(s)
ss = s * s
c = sqrt(1.0_wp - ss)
else
c = (w / r) * (1.0_wp - s2 * (a5 - u - c2 * v) / r)
lat = acos(c)
ss = 1.0_wp - c * c
s = sqrt(ss)
end if
g = 1.0_wp - e2 * ss
rg = a / sqrt(g)
rf = a6 * rg
u = w - rg * c
v = zp - rf * s
f = c * u + s * v
m = c * v - s * u
p = m / (rf / g + f)
lat = lat + p
if (z < 0.0_wp) lat = -lat
h = f + m * p / 2.0_wp
lon = atan2( y, x )
end if
end subroutine olson
For comparison, Heikkinen's algorithm is:
pure subroutine heikkinen(rvec, a, b, h, lon, lat)
use, intrinsic :: iso_fortran_env, wp=>real64 !double precision
implicit none
real(wp),dimension(3),intent(in) :: rvec !position vector [km]
real(wp),intent(in) :: a ! geoid semimajor axis [km]
real(wp),intent(in) :: b ! geoid semiminor axis [km]
real(wp),intent(out) :: h ! geodetic altitude [km]
real(wp),intent(out) :: lon ! longitude [rad]
real(wp),intent(out) :: lat ! geodetic latitude [rad]
real(wp) :: f,e_2,ep,r,e2,ff,g,c,s,pp,q,r0,u,v,z0,x,y,z,z2,r2,tmp,a2,b2
x = rvec(1)
y = rvec(2)
z = rvec(3)
a2 = a*a
b2 = b*b
f = (a-b)/a
e_2 = (2.0_wp*f-f*f)
ep = sqrt(a2/b2 - 1.0_wp)
z2 = z*z
r = sqrt(x**2 + y**2)
r2 = r*r
e2 = a2 - b2
ff = 54.0_wp * b2 * z2
g = r2 + (1.0_wp - e_2)*z2 - e_2*e2
c = e_2**2 * ff * r2 / g**3
s = (1.0_wp + c + sqrt(c**2 + 2.0_wp*c))**(1.0_wp/3.0_wp)
pp = ff / ( 3.0_wp*(s + 1.0_wp/s + 1.0_wp)**2 * g**2 )
q = sqrt( 1.0_wp + 2.0_wp*e_2**2 * pp )
r0 = -pp*e_2*r/(1.0_wp+q) + &
sqrt( max(0.0_wp, 1.0_wp/2.0_wp * a2 * (1.0_wp + 1.0_wp/q) - &
( pp*(1.0_wp-e_2)*z2 )/(q*(1.0_wp+q)) - &
1.0_wp/2.0_wp * pp * r2) )
u = sqrt( (r - e_2*r0)**2 + z2 )
v = sqrt( (r - e_2*r0)**2 + (1.0_wp - e_2)*z2 )
z0 = b**2 * z / (a*v)
h = u*(1.0_wp - b2/(a*v) )
lat = atan2( (z + ep**2*z0), r )
lon = atan2( y, x )
end subroutine heikkinen
Olson's is about 1.5 times faster on my 2.53 GHz i5 laptop, when both are compiled using gfortran with -O2 optimization (Olson: 3,743,212 cases/sec, Heikkinen: 2,499,670 cases/sec). The level of accuracy is about the same for each method.
References
- M. Heikkinen, "Geschlossene formeln zur berechnung raumlicher geodatischer koordinaten aus rechtwinkligen Koordinaten". Z. Ermess., 107 (1982), 207-211 (in German).
- E. D. Kaplan, "Understanding GPS: Principles and Applications", Artech House, 1996.
- D. K. Olson, "Converting Earth-Centered, Earth-Fixed Coordinates to Geodetic Coordinates," IEEE Transactions on Aerospace and Electronic Systems, 32 (1996) 473-476.