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 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 ;
endC++版本
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;
}
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
