经纬高转ENU matlab的代码

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

function [x,y,z] = llh2enu(lat,lon,h,lat0,lon0,h0)
% LLH2ENU Converts geodetic coordinates to local East, North, Up (ENU) 
%         coordinates.
%   [x,y,z] = LLH2ENU(lat,lon,h,lat0,lon0,h0) converts geodetic coordinates
%   (latitude, longitude, and height) to local ENU coordinates (East, North,
%   Up) given a local reference point (lat0, lon0, h0). All inputs are in
%   degrees and meters.
%
%   Reference: "A Guide to Coordinate Systems in Great Britain" by
%              Peter Osborne, 2011.
%
%   Example:
%       % Convert coordinates of the Eiffel Tower to ENU coordinates
%       lat = 48.858093; % N
%       lon = 2.294694; % E
%       h = 37; % meters
%       lat0 = 48.858093; % N
%       lon0 = 2.294694; % E
%       h0 = 0; % meters
%       [x,y,z] = llh2enu(lat,lon,h,lat0,lon0,h0);
%       fprintf('East: %.2f meters\n', x);
%       fprintf('North: %.2f meters\n', y);
%       fprintf('Up: %.2f meters\n', z);
%
%   See also ECEF2LLH, ECEF2ENU, ENU2ECEF, ENU2LLH.

% WGS84 ellipsoid constants
a = 6378137; % semi-major axis (meters)
b = 6356752.314245; % semi-minor axis (meters)
e = sqrt(1 - (b/a)^2); % eccentricity

% Convert to radians
lat = deg2rad(lat);
lon = deg2rad(lon);
lat0 = deg2rad(lat0);
lon0 = deg2rad(lon0);

% Radius of curvature in the prime vertical
N = a / sqrt(1 - e^2 * sin(lat0)^2);

% Cartesian coordinates of local reference point
x0 = (N + h0) * cos(lat0) * cos(lon0);
y0 = (N + h0) * cos(lat0) * sin(lon0);
z0 = ((1 - e^2) * N + h0) * sin(lat0);

% Cartesian coordinates of point to convert
x = (N + h) * cos(lat) * cos(lon);
y = (N + h) * cos(lat) * sin(lon);
z = ((1 - e^2) * N + h) * sin(lat);

% Rotation matrix from ECEF to ENU
R = [-sin(lon0)             cos(lon0)              0   ; ...
     -sin(lat0)*cos(lon0)  -sin(lat0)*sin(lon0)   cos(lat0); ...
      cos(lat0)*cos(lon0)   cos(lat0)*sin(lon0)   sin(lat0)];

% Translation vector from ECEF to ENU
T = [x0; y0; z0];

% Convert to ENU coordinates
P = R * ([x; y; z] - T);
x = P(1);
y = P(2);
z = P(3);
end