|
1 | | -function [lat,lon,alt] = ecef2geodetic(spheroid, x, y, z, angleUnit) |
| 1 | +function [lat,lon,alt] = ecef2geodetic(spheroid, x, y, z, angleUnit) |
2 | 2 | %ecef2geodetic convert ECEF to geodetic coordinates |
3 | 3 | % |
4 | 4 | % Inputs |
|
13 | 13 | % |
14 | 14 | % also see: http://www.oc.nps.edu/oc2902w/coord/coordcvt.pdf |
15 | 15 | % Fortran reference at bottom of: http://www.astro.uni.torun.pl/~kb/Papers/geod/Geod-BG.htm |
| 16 | +narginchk(3,5) |
| 17 | +if isempty(spheroid) |
| 18 | + spheroid = wgs84Ellipsoid(); |
| 19 | +elseif isnumeric(spheroid) && nargin == 3 |
| 20 | + z = y; |
| 21 | + y = x; |
| 22 | + x = spheroid; |
| 23 | + spheroid = wgs84Ellipsoid(); |
| 24 | +elseif isnumeric(spheroid) && ischar(z) && nargin == 4 |
| 25 | + angleUnit = z; |
| 26 | + z = y; |
| 27 | + y = x; |
| 28 | + x = spheroid; |
| 29 | + spheroid = wgs84Ellipsoid(); |
| 30 | +end |
16 | 31 |
|
17 | | - if isempty(spheroid), spheroid = wgs84Ellipsoid(); end |
| 32 | +if nargin < 5 || isempty(angleUnit), angleUnit='d'; end |
18 | 33 |
|
19 | | - % Algorithm is based on |
20 | | - % http://www.astro.uni.torun.pl/~kb/Papers/geod/Geod-BG.htm |
21 | | - % This algorithm provides a converging solution to the latitude equation |
22 | | - % in terms of the parametric or reduced latitude form (v) |
23 | | - % This algorithm provides a uniform solution over all latitudes as it does |
24 | | - % not involve division by cos(phi) or sin(phi) |
25 | | - a = spheroid.SemimajorAxis; |
26 | | - b = spheroid.SemiminorAxis; |
27 | | - r = hypot(x, y); |
28 | | - % Constant required for Latitude equation |
29 | | - rho = atan2(b * z, a * r); |
30 | | - % Constant required for latitude equation |
31 | | - c = (a^2 - b^2) ./ hypot(a*r, b*z); |
32 | | - count = 0; |
| 34 | +validateattributes(spheroid,{'struct'},{'nonempty'}) |
| 35 | +validateattributes(x, {'numeric'}, {'real'}) |
| 36 | +validateattributes(y, {'numeric'}, {'real'}) |
| 37 | +validateattributes(z, {'numeric'}, {'real'}) |
| 38 | +validateattributes(angleUnit,{'string','char'},{'scalar'}) |
| 39 | + |
| 40 | +%% compute |
| 41 | + |
| 42 | +% Algorithm is based on |
| 43 | +% http://www.astro.uni.torun.pl/~kb/Papers/geod/Geod-BG.htm |
| 44 | +% This algorithm provides a converging solution to the latitude equation |
| 45 | +% in terms of the parametric or reduced latitude form (v) |
| 46 | +% This algorithm provides a uniform solution over all latitudes as it does |
| 47 | +% not involve division by cos(phi) or sin(phi) |
| 48 | +a = spheroid.SemimajorAxis; |
| 49 | +b = spheroid.SemiminorAxis; |
| 50 | +r = hypot(x, y); |
| 51 | +% Constant required for Latitude equation |
| 52 | +rho = atan2(b * z, a * r); |
| 53 | +% Constant required for latitude equation |
| 54 | +c = (a^2 - b^2) ./ hypot(a*r, b*z); |
| 55 | +count = 0; |
33 | 56 | % Starter for the Newtons Iteration Method |
34 | | - vnew = atan2(a * z, b * r); |
| 57 | +vnew = atan2(a * z, b * r); |
35 | 58 | % Initializing the parametric latitude |
36 | | - v = 0; |
37 | | - while v ~= vnew && count < 5 |
38 | | - % disp([v,vnew]) |
39 | | - v = vnew; |
40 | | - % Derivative of latitude equation |
41 | | - w = 2 * (cos (v - rho) - c .* cos(2 * v)); |
42 | | - % Newtons Method for computing iterations |
43 | | - vnew = v - ((2 * sin (v - rho) - c .* sin(2 * v)) ./ w); |
44 | | - count = count+1; |
45 | | - end %while |
| 59 | +v = 0; |
| 60 | +while all(v ~= vnew) && count < 5 |
| 61 | +% disp([v,vnew]) |
| 62 | + v = vnew; |
| 63 | +% Derivative of latitude equation |
| 64 | +w = 2 * (cos (v - rho) - c .* cos(2 * v)); |
| 65 | +% Newtons Method for computing iterations |
| 66 | +vnew = v - ((2 * sin (v - rho) - c .* sin(2 * v)) ./ w); |
| 67 | +count = count+1; |
| 68 | +end %while |
46 | 69 |
|
47 | 70 | %% Computing latitude from the root of the latitude equation |
48 | | - lat = atan2(a * tan (vnew), b); |
49 | | - % Computing longitude |
50 | | - lon = atan2(y, x); |
51 | | - % Computing h from latitude obtained |
52 | | - alt = ((r - a * cos (vnew)) .* cos (lat)) + ... |
53 | | - ((z - b * sin (vnew)) .* sin (lat)); |
54 | | - |
55 | | - if nargin < 5 || isempty(angleUnit) || strcmpi(angleUnit(1),'d') |
56 | | - lat = rad2deg(lat); |
57 | | - lon = rad2deg(lon); |
58 | | - end |
| 71 | +lat = atan2(a * tan (vnew), b); |
| 72 | +% Computing longitude |
| 73 | +lon = atan2(y, x); |
| 74 | +% Computing h from latitude obtained |
| 75 | +alt = ((r - a * cos (vnew)) .* cos (lat)) + ... |
| 76 | + ((z - b * sin (vnew)) .* sin (lat)); |
| 77 | + |
| 78 | +if strcmpi(angleUnit(1),'d') |
| 79 | + lat = rad2deg(lat); |
| 80 | + lon = rad2deg(lon); |
| 81 | +end |
59 | 82 |
|
60 | 83 | end % function |
61 | 84 |
|
|
0 commit comments