ECEF to lla (lat,lon,alt) in java

Question

Ive looked through the posts in the site and haven't found my problem ... as the head line says im trying to convert from ecef to lla .

I'm using this document : Conversion article in the direct formula , not the iterate formula and this site for result comparison : ECEF2LLA

Im developing in java so my code is as follows :

public static final double a = 6378137;
public static final double f = 1/298.257223563;
public static final double b = a*(1-f);
public static final double e = Math.sqrt((Math.pow(a, 2)-Math.pow(b, 2))/Math.pow(a, 2));
public static final double e2 = Math.sqrt((Math.pow(a, 2)-Math.pow(b, 2))/Math.pow(b, 2));
public static double[] ecef2lla(double x , double y , double z){
    double[] lla = {0,0,0};
    double lon,lat,height,N;
    double p = Math.sqrt(Math.pow(x, 2)+Math.pow(y, 2));
    double theta = Math.atan((z*a)/(p*b));
    lon = Math.atan(y/x);
    lon = lon*180/Math.PI;

    lat = Math.atan(((z+Math.pow(e2, 2)*b*Math.pow(Math.sin(theta), 3))/((p-Math.pow(e,2)*a*Math.pow(Math.cos(theta), 3)))));
    lat = (lat*180)/Math.PI;

    N= a/(Math.sqrt(1-Math.pow(e*Math.sin(lat), 2)));
    height = (p/Math.cos(theta)) - N;
    lla[0] = lon;
    lla[1] = lat;
    lla[2] = height;
    return lla;

}

I'm getting wrong height data. I've tried to move to radians and degrees and what not .

Thank you in advance !

Answer 1

I found this post and was ready to use the "accepted answer" in part of my application but I decided to run a couple of tests first to validate the algorithm. I used sample data generated by an online conversion calculator (http://www.sysense.com/products/ecef_lla_converter/index.html). I got less than stellar results as shown by the output below.

-----Test 1---------
Inputs:   -576793.17, -5376363.47, 3372298.51
Expected: 32.12345, -96.12345, 500.0
Actuals:  32.12306332822881, 83.87654999786477, 486.5472474489361
-----Test 2---------
Inputs:   2297292.91, 1016894.94, -5843939.62
Expected: -66.87654, 23.87654, 1000.0
Actuals:  -66.876230479461, 23.87653991401422, 959.6879360172898

Then I reran the same tests using the code from the following post (https://gist.github.com/klucar/1536194) and got much better results as shown by the output below.

-----Test 1---------
Inputs:   -576793.17, -5376363.47, 3372298.51
Expected: 32.12345, -96.12345, 500.0
Actuals:  32.12345004807767, -96.12345000213524, 499.997958839871
-----Test 2---------
Inputs:   2297292.91, 1016894.94, -5843939.62
Expected: -66.87654, 23.87654, 1000.0
Actuals:  -66.87654001741278, 23.87653991401422, 999.9983866894618

I didn't take the time to find the error in the solution provided in the "accepted answer" but my suggested answer: Use this code...

/*
*
*  ECEF - Earth Centered Earth Fixed
*   
*  LLA - Lat Lon Alt
*
*  ported from matlab code at
*  https://gist.github.com/1536054
*     and
*  https://gist.github.com/1536056
*/

// WGS84 ellipsoid constants
private final double a = 6378137; // radius
private final double e = 8.1819190842622e-2;  // eccentricity

private final double asq = Math.pow(a,2);
private final double esq = Math.pow(e,2);

private double[] ecef2lla(double[] ecef){
  double x = ecef[0];
  double y = ecef[1];
  double z = ecef[2];

  double b = Math.sqrt( asq * (1-esq) );
  double bsq = Math.pow(b,2);
  double ep = Math.sqrt( (asq - bsq)/bsq);
  double p = Math.sqrt( Math.pow(x,2) + Math.pow(y,2) );
  double th = Math.atan2(a*z, b*p);

  double lon = Math.atan2(y,x);
  double lat = Math.atan2( (z + Math.pow(ep,2)*b*Math.pow(Math.sin(th),3) ), (p - esq*a*Math.pow(Math.cos(th),3)) );
  double N = a/( Math.sqrt(1-esq*Math.pow(Math.sin(lat),2)) );
  double alt = p / Math.cos(lat) - N;

  // mod lat to 0-2pi
  lon = lon % (2*Math.PI);

  // correction for altitude near poles left out.

  double[] ret = {lat, lon, alt};

  return ret;
}