东北天、地心地固、WGS84转换-代码

matlab\\C++\\python版本，均与matlab自带函数进行过对比

前段时间项目中因为涉及不同目标在地球大环境下的搜寻问题，需要针对不同目标计算各种坐标系的转换，最终统一放到地球WGS84坐标系下进行计算。当时找了很多没有发现C++版本的函数，而且一些CSDN中讲的近似公式过于简单且参数不明确，结果不对。这个版本的结果目前已经和matlab中的函数进行过对比，没有问题，正好记录下来，方便自己和大家使用。

对于几种不同的坐标系，已经有很多讲解，我就不细说了。简单说下我的理解，WGS84，也就是地理坐标系 (Geographic Coordinate System, GCS)，就是经纬高来表示的，每个静止目标的经纬高是固定且唯一的。

地心地固直角坐标系（ECEF），其 x 轴延伸通过本初子午线（经度 0 度）和赤道（0度纬度）。z 轴延伸穿过真正的北极（即与地球自转轴重合）。y 轴完成右手坐标系，穿过赤道和 90 度经度。其实就是以地心为原点的直角坐标系，xyz表示，每个静止目标的位置是固定且唯一的。

东北天（ENU），是局部坐标系，原点为自己设置的一个目标，比如某个灯塔或者舰船，东北天分别对应当下这个坐标系的xyz（和地心地固的xyz无关，不要混淆）。每个静止目标的位置根据原点目标的不同而不同。

matlab自带函数

wgs84 = wgs84Ellipsoid;
[ship_Em,ship_Nm,ship_Um] = geodetic2enu(ship_Lat,ship_Lon,ship_Alt,ship_Lat,ship_Lon,ship_Alt,wgs84);

matlab中更多的地理坐标系转换函数：

doc geodetic2enu % 地理转东北天
doc ecef2enu % 地心地固转东北天
doc enu2geodetic % 东北天转地理
doc geodetic2aer % 地理转我不知道我没用过
doc geodetic2ned % 地理转我不知道我没用过
%% 更多的可以doc再去搜

自带函数的调用测试如下，其中第12行中wgs84 = wgs84Ellipsoid;表示调用的WGS84坐标系。

%% matlab测试 东北天 导引头 船的位置
clear;clc;close all;tic;
%% 初始化位置
ship_Lon = 117.69;   % 经度
ship_Lat = 12.0487;   % 纬度
ship_Alt = 0;         % 高度
seeker_Lon = 117.661;   % 经度
seeker_Lat = 13.2119;   % 纬度
seeker_Alt = 25532.2;     % 高度%%
wgs84 = wgs84Ellipsoid;
[ship_Em,ship_Nm,ship_Um] = geodetic2enu(ship_Lat,ship_Lon,ship_Alt,ship_Lat,ship_Lon,ship_Alt,wgs84);
[seeker_Em,seeker_Nm,seeker_Um] = geodetic2enu(seeker_Lat,seeker_Lon,seeker_Alt,ship_Lat,ship_Lon,ship_Alt,wgs84);
distance_enu_matlab =sqrt((seeker_Em-ship_Em)^2 +(seeker_Nm-ship_Nm)^2 +(seeker_Um-ship_Um)^2 );
string_ship1 =   ['船     经 ',num2str(ship_Lon),';  纬 ',num2str(ship_Lat),'; 高  ',num2str(ship_Alt)]; disp(string_ship1);
string_seeker1 = ['导引头 经 ',num2str(seeker_Lon),';  纬 ',num2str(seeker_Lat),';  高  ',num2str(seeker_Alt)]; disp(string_seeker1);
disp(' ');
string_ship2 =   ['船     东 ',num2str(ship_Em),';  北 ',num2str(ship_Nm),'; 天  ',num2str(ship_Um)]; disp(string_ship2);
string_seeker2 = ['导引头 东 ',num2str(seeker_Em),';  北 ',num2str(seeker_Nm),';  天  ',num2str(seeker_Um)]; disp(string_seeker2);

matlab自编版本

地理坐标系转地心地固坐标系，输入为 经纬高，输出为x y z

function [x, y, z] = geodetic_to_ecef(lat, lon, h)a = 6378137;b = 6356752.3142;f = (a - b) / a;e_sq = f * (2-f);lamb = lat/180.0*pi;  % 角度换成弧度phi = lon/180.0*pi; s = sin(lamb);N = a / sqrt(1 - e_sq * s * s);sin_lambda = sin(lamb);cos_lambda = cos(lamb);sin_phi = sin(phi);cos_phi = cos(phi);x = (h + N) * cos_lambda * cos_phi;y = (h + N) * cos_lambda * sin_phi;z = (h + (1 - e_sq) * N) * sin_lambda;
end

地心地固坐标系转东北天坐标系，输入为 地心地固xyz，局部坐标系原点的经纬高，输出为东北天的x y z

function [xEast, yNorth, zUp] = ecef_to_enu(x, y, z, lat0, lon0, h0)a = 6378137;b = 6356752.3142;f = (a - b) / a;e_sq = f * (2-f);lamb =lat0/180.0*pi; phi = lon0/180.0*pi; s = sin(lamb);N = a / sqrt(1 - e_sq * s * s);sin_lambda = sin(lamb);cos_lambda = cos(lamb);sin_phi = sin(phi);cos_phi = cos(phi);x0 = (h0 + N) * cos_lambda * cos_phi;y0 = (h0 + N) * cos_lambda * sin_phi;z0 = (h0 + (1 - e_sq) * N) * sin_lambda;xd = x - x0;yd = y - y0;zd = z - z0;t = -cos_phi * xd -  sin_phi * yd;xEast = -sin_phi * xd + cos_phi * yd;yNorth = t * sin_lambda  + cos_lambda * zd;zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd;
end

地理坐标系转东北天坐标系，输入为 目标的经纬高，局部坐标系原点的经纬高，输出为东北天的x y z

function [xEast, yNorth, zUp] = geodetic_to_enu(lat, lon, h,lat0, lon0, h0)a = 6378137;b = 6356752.3142;f = (a - b) / a;e_sq = f * (2-f);%%lamb = lat/180.0*pi;  % 角度换成弧度phi = lon/180.0*pi; s = sin(lamb);N = a / sqrt(1 - e_sq * s * s);sin_lambda = sin(lamb);cos_lambda = cos(lamb);sin_phi = sin(phi);cos_phi = cos(phi);x = (h + N) * cos_lambda * cos_phi;y = (h + N) * cos_lambda * sin_phi;z = (h + (1 - e_sq) * N) * sin_lambda;%% 原点坐标转换lamb0 =lat0/180.0*pi; phi0 = lon0/180.0*pi; s0 = sin(lamb0);N0 = a / sqrt(1 - e_sq * s0 * s0);sin_lambda0 = sin(lamb0);cos_lambda0 = cos(lamb0);sin_phi0 = sin(phi0);cos_phi0 = cos(phi0);x0 = (h0 + N0) * cos_lambda0 * cos_phi0;y0 = (h0 + N0) * cos_lambda0 * sin_phi0;z0 = (h0 + (1 - e_sq) * N0) * sin_lambda0;    %%xd = x - x0;yd = y - y0;zd = z - z0;t = -cos_phi0 * xd -  sin_phi0 * yd;xEast = -sin_phi0 * xd + cos_phi0 * yd;yNorth = t * sin_lambda0  + cos_lambda0 * zd;zUp = cos_lambda0 * cos_phi0 * xd + cos_lambda0 * sin_phi0 * yd + sin_lambda0 * zd;
end

东北天坐标系转地心地固坐标系，输入为目标在东北天中的xyz，以及东北天原点的经纬高，输出为输出为地心地固的x y z

function [x, y, z]=enu_to_ecef(xEast, yNorth, zUp, lat0, lon0, h0)
a = 6378137;
b = 6356752.3142;
f = (a - b) / a;
e_sq = f * (2-f);
pi = 3.14159265359;lamb = pi/180*(lat0);
phi = pi/180*(lon0);
s =  sin(lamb);
N = a /  sqrt(1 - e_sq * s * s);sin_lambda =  sin(lamb);
cos_lambda =  cos(lamb);
sin_phi =  sin(phi);
cos_phi =  cos(phi);x0 = (h0 + N) * cos_lambda * cos_phi;
y0 = (h0 + N) * cos_lambda * sin_phi;
z0 = (h0 + (1 - e_sq) * N) * sin_lambda;t = cos_lambda * zUp - sin_lambda * yNorth;zd = sin_lambda * zUp + cos_lambda * yNorth;
xd = cos_phi * t - sin_phi * xEast ;
yd = sin_phi * t + cos_phi * xEast;x = xd + x0 ;
y = yd + y0 ;
z = zd + z0 ;
end

东北天坐标系转地理坐标系，输入为目标在东北天中的xyz，原点的经纬高，输出为目标的经纬高

function [lat, lon, h] = enu_to_geodetic(xEast, yNorth, zUp, lat_ref, lon_ref, h_ref)[x,y,z] = enu_to_ecef(xEast, yNorth, zUp, lat_ref, lon_ref, h_ref);[lat, lon, h] =ecef_to_geodetic(x,y,z);
end

地心地固转地理坐标系，输入为地心地固x y z，输出为地理的经纬高

function [lat0, lon0, h0]=ecef_to_geodetic(x, y, z)
%    # Convert from ECEF cartesian coordinates to 
%    # latitude, longitude and height.  WGS-84                a = 6378137;b = 6356752.3142;f = (a - b) / a;e_sq = f * (2-f);pi = 3.14159265359;x2 = x ^ 2 ;y2 = y ^ 2 ;z2 = z ^ 2 ;a = 6378137.0000;    %earth radius in metersb = 6356752.3142 ;   %earth semiminor in meters e =  sqrt (1-(b/a)^2); b2 = b*b ;e2 = e ^ 2; ep = e*(a/b); r =  sqrt(x2+y2); r2 = r*r ;E2 = a ^ 2 - b ^ 2; F = 54*b2*z2 ;G = r2 + (1-e2)*z2 - e2*E2 ;c = (e2*e2*F*r2)/(G*G*G) ;s = ( 1 + c +  sqrt(c*c + 2*c) )^(1/3) ;P = F / (3 * (s+1/s+1)^2 * G*G) ;Q =  sqrt(1+2*e2*e2*P) ;ro = -(P*e2*r)/(1+Q) +  sqrt((a*a/2)*(1+1/Q) - (P*(1-e2)*z2)/(Q*(1+Q)) - P*r2/2); tmp = (r - e2*ro) ^ 2 ;U =  sqrt( tmp + z2 ) ;V =  sqrt( tmp + (1-e2)*z2 ) ;zo = (b2*z)/(a*V) ;height = U*( 1 - b2/(a*V) ); lat =  atan( (z + ep*ep*zo)/r ) ;temp =  atan(y/x) ;if x >=0     long = temp ;elseif (x < 0) & (y >= 0)long = pi + temp ;else long = temp - pi ;endendlat0 = lat/(pi/180) ;lon0 = long/(pi/180) ;h0 = height ;
end

C++版本

C++版本只写了这两个，其他的可以根据matlab自己改写。

//  WGS84  转 东北天 ENU
//  lat, lon在程序中为弧度，并非角度，调用前需要自己转换 
void GeoDetic_TO_ENU(double lat, double lon, double h, double lat0, double lon0, double h0, double enu_xyz[3])
{double a, b, f, e_sq;pi = 3.14159265359;a = 6378137;b = 6356752.3142;f = (a - b) / a;e_sq = f * (2 - f);// 站点（非原点）double lamb, phi, s, N, sin_lambda, cos_lambda, sin_phi, cos_phi, x, y, z;lamb = lat;  phi = lon;s = sin(lamb);N = a / sqrt(1 - e_sq * s * s);sin_lambda = sin(lamb);cos_lambda = cos(lamb);sin_phi = sin(phi);cos_phi = cos(phi);x = (h + N) * cos_lambda * cos_phi;y = (h + N) * cos_lambda * sin_phi;z = (h + (1 - e_sq) * N) * sin_lambda;// 原点坐标转换double lamb0, phi0, s0, N0, sin_lambda0, cos_lambda0, sin_phi0, cos_phi0, x0, y0, z0;lamb0 = lat0;phi0 = lon0;s0 = sin(lamb0);N0 = a / sqrt(1 - e_sq * s0 * s0);sin_lambda0 = sin(lamb0);cos_lambda0 = cos(lamb0);sin_phi0 = sin(phi0);cos_phi0 = cos(phi0);x0 = (h0 + N0) * cos_lambda0 * cos_phi0;y0 = (h0 + N0) * cos_lambda0 * sin_phi0;z0 = (h0 + (1 - e_sq) * N0) * sin_lambda0;// ECEF 转 ENUdouble xd, yd, zd, t;xd = x - x0;yd = y - y0;zd = z - z0;t = -cos_phi0 * xd - sin_phi0 * yd;enu_xyz[0] = -sin_phi0 * xd + cos_phi0 * yd;enu_xyz[1] = t * sin_lambda0 + cos_lambda0 * zd;enu_xyz[2] = cos_lambda0 * cos_phi0 * xd + cos_lambda0 * sin_phi0 * yd + sin_lambda0 * zd;
}// 东北天 ENU 转 地心地固坐标系 ECEF
//  lat0, lon0在程序中为弧度，并非角度，调用前需要自己转换 
void ENU_TO_ECEF(double xEast, double yNorth, double zUp, double lat0, double lon0, double h0, double enu_ecef[3])
{double a, b, f, e_sq, pii;a = 6378137;b = 6356752.3142;f = (a - b) / a;e_sq = f * (2 - f);pii = 3.14159265359;double lamb, phi, s, N, sin_lambda, cos_lambda, sin_phi, cos_phi;lamb = lat0;phi = lon0;s = sin(lamb);N = a / sqrt(1 - e_sq * s * s);sin_lambda = sin(lamb);cos_lambda = cos(lamb);sin_phi = sin(phi);cos_phi = cos(phi);double x0, y0, z0, t, xd, yd, zd;x0 = (h0 + N) * cos_lambda * cos_phi;y0 = (h0 + N) * cos_lambda * sin_phi;z0 = (h0 + (1 - e_sq) * N) * sin_lambda;t = cos_lambda * zUp - sin_lambda * yNorth;zd = sin_lambda * zUp + cos_lambda * yNorth;xd = cos_phi * t - sin_phi * xEast;yd = sin_phi * t + cos_phi * xEast;enu_ecef[0] = xd + x0;enu_ecef[1] = yd + y0;enu_ecef[2] = zd + z0;
}