东北天、地心地固、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自编版本

1. 地理坐标系转地心地固坐标系，输入为 经纬高，输出为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

2. 地心地固坐标系转东北天坐标系，输入为 地心地固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	

3. 地理坐标系转东北天坐标系，输入为 目标的经纬高，局部坐标系原点的经纬高，输出为东北天的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

4. 东北天坐标系转地心地固坐标系，输入为目标在东北天中的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

5. 东北天坐标系转地理坐标系，输入为目标在东北天中的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

6. 地心地固转地理坐标系，输入为地心地固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 meters
    b = 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 ;
    else
        if (x < 0) & (y >= 0)
            long = pi + temp ;
        else 
            long = temp - pi ;
        end
    end

    lat0 = 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 转 ENU
	double 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;
}

python版本

python版本是CSDN一个大佬的，原文件链接。
Python版本