# 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) :: 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) :: 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

1. M. Heikkinen, “Geschlossene formeln zur berechnung raumlicher geodatischer koordinaten aus rechtwinkligen Koordinaten”. Z. Ermess., 107 (1982), 207-211 (in German).
2. E. D. Kaplan, “Understanding GPS: Principles and Applications”, Artech House, 1996.
3. D. K. Olson, “Converting Earth-Centered, Earth-Fixed Coordinates to Geodetic Coordinates,” IEEE Transactions on Aerospace and Electronic Systems, 32 (1996) 473-476.
Tagged with: ,