东北天（ENU）转经纬高(LLA)

经纬高分别为lon ,lat, alt  度数为弧度

东北天xyz为enupoint.x，enupoint.y，enupoint.z

地心地固直角坐标系（ECEF）xyz为ecefpoint.x，ecefpoint.y，ecefpoint.z

1、东北天（ENU）转地心地固直角坐标系（ECEF）

double cosLat,sinLat,cosLon,sinLon;    

cosLat = cos(lat);

sinLat = sin(lat);
cosLon = cos(lon);
sinLon = sin(lon);
ecefpoint.x=-sinLon*enupoint.x-sinLat*cosLon*enupoint.y+cosLat*cosLon*enupoint.z;
 ecefpoint.y=cosLon*enupoint.x-sinLat*sinLon*enupoint.y+cosLat*sinLon*enupoint.z;
  ecefpoint.z=cosLat*enupoint.y+sinLat*enupoint.z;

2、地心地固直角坐标系（ECEF）转经纬高（LLA）

double WGS84_A = 6378137.0;      // major axis
    double WGS84_E = 0.081819190842622;   // first eccentricity

    double b = sqrt(WGS84_A * WGS84_A * (1 - WGS84_E * WGS84_E));
    double ep = sqrt((WGS84_A * WGS84_A - b * b) / (b * b));
    double p = hypot(coord.x, coord.y);
    double th = atan2(WGS84_A * coord.z, b * p);
    double lon = atan2(coord.y, coord.x);
    double lat = atan2((coord.z + ep * ep * b * pow(sin(th), 3)), (p - WGS84_E * WGS84_E * WGS84_A * pow(cos(th), 3)));
    double N = WGS84_A / sqrt(1 - WGS84_E * WGS84_E * sin(lat) * sin(lat));
    double alt = p / cos(lat) - N;
    latitude=lat*180/PI;
    longitude=lon*180/PI;
    height=alt;