|
|
|
|
|
function [llaDegDegM] = Xyz2Lla(xyzM)
|
|
|
|
|
|
% [llaDegDegM] = Xyz2Lla(xyzM)
|
|
|
|
|
|
%
|
|
|
|
|
|
% Transform Earth-Centered-Earth-Fixed x,y,z, coordinates to lat,lon,alt.
|
|
|
|
|
|
% Input: xyzM = [mx3] matrix of ECEF coordinates in meters
|
|
|
|
|
|
% Output: llaDegDegM = [mx3] matrix = [latDeg,lonDeg,altM]
|
|
|
|
|
|
% latitude, longitude are returned in degrees and altitude in meters
|
|
|
|
|
|
%
|
|
|
|
|
|
% See also Lla2Xyz
|
|
|
|
|
|
|
|
|
|
|
|
%Author: Frank van Diggelen
|
|
|
|
|
|
%Open Source code for processing Android GNSS Measurements
|
|
|
|
|
|
|
|
|
|
|
|
% check inputs
|
|
|
|
|
|
if size(xyzM,2)~=3
|
|
|
|
|
|
error('Input xyzM must have three columns');
|
|
|
|
|
|
end
|
|
|
|
|
|
% algorithm: Hoffman-Wellenhof, Lichtenegger & Collins "GPS Theory & Practice"
|
|
|
|
|
|
R2D = 180/pi;
|
|
|
|
|
|
|
|
|
|
|
|
%if x and y ecef positions are both zero then lla is undefined
|
|
|
|
|
|
iZero = ( xyzM(:,1)==0 & xyzM(:,2)==0);
|
|
|
|
|
|
xyzM(iZero,:) = NaN; %set to NaN, so lla will also be NaN
|
|
|
|
|
|
|
|
|
|
|
|
xM = xyzM(:,1); yM = xyzM(:,2); zM = xyzM(:,3);
|
|
|
|
|
|
%following algorithm from Hoffman-Wellenhof, et al. "GPS Theory & Practice":
|
|
|
|
|
|
a = GpsConstants.EARTHSEMIMAJOR;
|
|
|
|
|
|
a2 = a^2;
|
|
|
|
|
|
b2 = a2*(1-GpsConstants.EARTHECCEN2);
|
|
|
|
|
|
b = sqrt(b2);
|
|
|
|
|
|
e2 = GpsConstants.EARTHECCEN2;
|
|
|
|
|
|
ep2 = (a2-b2)/b2;
|
|
|
|
|
|
p=sqrt(xM.^2 + yM.^2);
|
|
|
|
|
|
|
|
|
|
|
|
% two sides and hypotenuse of right angle triangle with one angle = theta:
|
|
|
|
|
|
s1 = zM*a;
|
|
|
|
|
|
s2 = p*b;
|
|
|
|
|
|
h = sqrt(s1.^2 + s2.^2);
|
|
|
|
|
|
sin_theta = s1./h;
|
|
|
|
|
|
cos_theta = s2./h;
|
|
|
|
|
|
%theta = atan(s1./s2);
|
|
|
|
|
|
|
|
|
|
|
|
% two sides and hypotenuse of right angle triangle with one angle = lat:
|
|
|
|
|
|
s1 = zM+ep2*b*(sin_theta.^3);
|
|
|
|
|
|
s2 = p-a*e2.*(cos_theta.^3);
|
|
|
|
|
|
h = sqrt(s1.^2 + s2.^2);
|
|
|
|
|
|
tan_lat = s1./s2;
|
|
|
|
|
|
sin_lat = s1./h;
|
|
|
|
|
|
cos_lat = s2./h;
|
|
|
|
|
|
latDeg = atan(tan_lat);
|
|
|
|
|
|
latDeg = latDeg*R2D;
|
|
|
|
|
|
|
|
|
|
|
|
N = a2*((a2*(cos_lat.^2) + b2*(sin_lat.^2)).^(-0.5));
|
|
|
|
|
|
altM = p./cos_lat - N;
|
|
|
|
|
|
|
|
|
|
|
|
% rotate longitude to where it would be for a fixed point in ECI
|
|
|
|
|
|
%lonDeg = atan2(yM,xM) - GpsConstants.WE*deltaTime;
|
|
|
|
|
|
lonDeg = atan2(yM,xM); %since deltaTime = 0 for ECEF
|
|
|
|
|
|
lonDeg = rem(lonDeg,2*pi)*R2D;
|
|
|
|
|
|
if (lonDeg>180)
|
|
|
|
|
|
lonDeg = lonDeg-360;
|
|
|
|
|
|
end
|
|
|
|
|
|
llaDegDegM = [latDeg, lonDeg,altM];
|
|
|
|
|
|
|
|
|
|
|
|
end %end of function Xyz2Lla
|
|
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
% Copyright 2016 Google Inc.
|
|
|
|
|
|
%
|
|
|
|
|
|
% Licensed under the Apache License, Version 2.0 (the "License");
|
|
|
|
|
|
% you may not use this file except in compliance with the License.
|
|
|
|
|
|
% You may obtain a copy of the License at
|
|
|
|
|
|
%
|
|
|
|
|
|
% http://www.apache.org/licenses/LICENSE-2.0
|
|
|
|
|
|
%
|
|
|
|
|
|
% Unless required by applicable law or agreed to in writing, software
|
|
|
|
|
|
% distributed under the License is distributed on an "AS IS" BASIS,
|
|
|
|
|
|
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
|
|
|
|
% See the License for the specific language governing permissions and
|
|
|
|
|
|
% limitations under the License.
|
|
|
|
|
|
|
