ECEF (Earth Centered Earth Fixed) Coords with WGS-4 Ellipsoid to Latitude Longitude Altitude

/*
*
* ECEF - Earth Centered Earth Fixed
*
* LLA - Lat Lon Alt
*
* ported from matlab code at
*/

// 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;
}

private double[] lla2ecef(double[] lla){
double lat = lla[0];
double lon = lla[1];
double alt = lla[2];

double N = a / Math.sqrt(1 – esq * Math.pow(Math.sin(lat),2) );

double x = (N+alt) * Math.cos(lat) * Math.cos(lon);
double y = (N+alt) * Math.cos(lat) * Math.sin(lon);
double z = ((1-esq) * N + alt) * Math.sin(lat);

double[] ret = {x, y, z};
return ret;
}

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s