经纬高转ENU matlab代码

以下是经纬高转ENU的Matlab代码：

function [East, North, Up] = ECEF2ENU(X, Y, Z, lat0, lon0, h0)
% ECEF2ENU Converts ECEF coordinates to local East-North-Up (ENU) coordinates
% [East, North, Up] = ECEF2ENU(X, Y, Z, lat0, lon0, h0) converts ECEF coordinates
% (X, Y, Z) to local East-North-Up (ENU) coordinates with respect to a reference
% point at (lat0, lon0, h0) using the WGS-84 ellipsoid. The inputs are in meters
% and degrees. The outputs are in meters.
%
% Note: The ENU coordinate system has its origin at (lat0, lon0, h0).
%
% Reference:
% [1] Hofmann-Wellenhof, B., Lichtenegger, H., & Collins, J. (1994).
% GPS: theory and practice (5th ed.). Springer-Verlag Wien New York.

% WGS-84 ellipsoid parameters
a = 6378137.0; % semi-major axis (m)
b = 6356752.3142; % semi-minor axis (m)
f = (a - b) / a; % flattening
e2 = f * (2 - f); % eccentricity squared

% Convert reference point to ECEF coordinates
[lat0, lon0, h0] = deg2rad([lat0, lon0, h0]);
N0 = a / sqrt(1 - e2 * sin(lat0)^2);
X0 = (N0 + h0) * cos(lat0) * cos(lon0);
Y0 = (N0 + h0) * cos(lat0) * sin(lon0);
Z0 = (N0 * (1 - e2) + h0) * sin(lat0);

% Convert ECEF coordinates to ENU coordinates
X = X - X0;
Y = Y - Y0;
Z = Z - Z0;
R = [-sin(lon0) cos(lon0) 0; ...
-cos(lon0)*sin(lat0) -sin(lat0)*sin(lon0) cos(lat0); ...
cos(lat0)*cos(lon0) cos(lat0)*sin(lon0) sin(lat0)];
ENU = R * [X; Y; Z];

% Extract East, North, Up coordinates
East = ENU(1);
North = ENU(2);
Up = ENU(3);

end