51 lines
1019 B
Mathematica
51 lines
1019 B
Mathematica
|
%
|
||
|
|
% llh = ecef2llh (ecef)
|
||
|
|
%
|
||
|
|
% ecef2llh: converts ECEF to Lat, Lon, Height in WGS-84
|
||
|
|
%
|
||
|
|
% Input: ecef [3xN] = x,y,z (user units)
|
||
|
|
% Output: llh [3xN] = lat,long,height location (rad,rad,user units)
|
||
|
|
%
|
||
|
|
% EE6900 - Flight Management Systems
|
||
|
|
%
|
||
|
|
% Maarten Uijt de Haag
|
||
|
|
%
|
||
|
|
% Based on: OU-ECE-AEC Oct. 1988 FvG
|
||
|
|
%
|
||
|
|
function llh = ecef2llh (ecef)
|
||
|
|
|
||
|
|
A = 6378137.0;
|
||
|
|
E = 8.1819190842622e-02;
|
||
|
|
ESQ = E*E;
|
||
|
|
|
||
|
|
llh = 0.0*ecef;
|
||
|
|
|
||
|
|
X = ecef(1,:);
|
||
|
|
Y = ecef(2,:);
|
||
|
|
Z = ecef(3,:);
|
||
|
|
|
||
|
|
RSQ = (X.*X) + (Y.*Y);
|
||
|
|
H = ESQ .* Z;
|
||
|
|
|
||
|
|
%cnt = 0;
|
||
|
|
%done = 0;
|
||
|
|
%while cnt < 10 & done == 0,
|
||
|
|
for cnt = 1:10,
|
||
|
|
%cnt = cnt + 1;
|
||
|
|
ZP = Z + H;
|
||
|
|
R = sqrt(RSQ + (ZP .* ZP));
|
||
|
|
SP = ZP ./ R;
|
||
|
|
GSQ = 1.0 - (ESQ .* SP .* SP);
|
||
|
|
EN = A ./ sqrt(GSQ);
|
||
|
|
P = EN .* ESQ .* SP;
|
||
|
|
%if abs(H-P) < 5.0e-04, done = 1; end;
|
||
|
|
H = P;
|
||
|
|
end;
|
||
|
|
P = atan2( ZP, sqrt(RSQ) );
|
||
|
|
H = R - EN;
|
||
|
|
llh(1,:) = P;
|
||
|
|
llh(2,:) = atan2(Y,X);
|
||
|
|
llh(3,:) = H;
|
||
|
|
|
||
|
|
return
|