From c9b2f06d410dbce3215d295ceeaa05abfd49cae6 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 30 Mar 2018 11:36:50 +0200 Subject: [PATCH] Clean up Matlab/Octave code --- src/utils/matlab/libs/geoFunctions/cart2geo.m | 36 ++++--- src/utils/matlab/libs/geoFunctions/cart2utm.m | 97 +++++++++---------- src/utils/matlab/libs/geoFunctions/check_t.m | 16 ++- src/utils/matlab/libs/geoFunctions/clksin.m | 6 +- src/utils/matlab/libs/geoFunctions/clsin.m | 18 ++-- src/utils/matlab/libs/geoFunctions/deg2dms.m | 8 +- src/utils/matlab/libs/geoFunctions/dms2deg.m | 6 +- src/utils/matlab/libs/geoFunctions/dms2mat.m | 6 +- src/utils/matlab/libs/geoFunctions/e_r_corr.m | 19 ++-- .../matlab/libs/geoFunctions/findUtmZone.m | 23 ++--- src/utils/matlab/libs/geoFunctions/geo2cart.m | 24 +++-- .../matlab/libs/geoFunctions/leastSquarePos.m | 41 ++++---- src/utils/matlab/libs/geoFunctions/mat2dms.m | 11 +-- src/utils/matlab/libs/geoFunctions/roundn.m | 6 +- src/utils/matlab/libs/geoFunctions/satpos.m | 87 ++++++++--------- src/utils/matlab/libs/geoFunctions/togeod.m | 9 +- src/utils/matlab/libs/geoFunctions/topocent.m | 23 ++--- src/utils/matlab/libs/geoFunctions/tropo.m | 23 ++--- 18 files changed, 212 insertions(+), 247 deletions(-) diff --git a/src/utils/matlab/libs/geoFunctions/cart2geo.m b/src/utils/matlab/libs/geoFunctions/cart2geo.m index 99888b12e..45947151f 100644 --- a/src/utils/matlab/libs/geoFunctions/cart2geo.m +++ b/src/utils/matlab/libs/geoFunctions/cart2geo.m @@ -1,8 +1,8 @@ function [phi, lambda, h] = cart2geo(X, Y, Z, i) -%CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical -%coordinates (phi, lambda, h) on a selected reference ellipsoid. +% CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical +% coordinates (phi, lambda, h) on a selected reference ellipsoid. % -%[phi, lambda, h] = cart2geo(X, Y, Z, i); +% [phi, lambda, h] = cart2geo(X, Y, Z, i); % % Choices i of Reference Ellipsoid for Geographical Coordinates % 1. International Ellipsoid 1924 @@ -11,12 +11,9 @@ function [phi, lambda, h] = cart2geo(X, Y, Z, i) % 4. Geodetic Reference System 1980 % 5. World Geodetic System 1984 -%Kai Borre 10-13-98 -%Copyright (c) by Kai Borre -%Revision: 1.0 Date: 1998/10/23 -% -% CVS record: -% $Id: cart2geo.m,v 1.1.2.3 2007/01/29 15:22:49 dpl Exp $ +% Kai Borre 10-13-98 +% Copyright (c) by Kai Borre +% Revision: 1.0 Date: 1998/10/23 %========================================================================== a = [6378388 6378160 6378135 6378137 6378137]; @@ -30,16 +27,16 @@ phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i)))*f(i)))); h = 0.1; oldh = 0; iterations = 0; while abs(h-oldh) > 1.e-12 - oldh = h; - N = c/sqrt(1+ex2*cos(phi)^2); - phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i))*f(i)*N/(N+h))))); - h = sqrt(X^2+Y^2)/cos(phi)-N; - - iterations = iterations + 1; - if iterations > 100 - fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh); - break; - end + oldh = h; + N = c/sqrt(1+ex2*cos(phi)^2); + phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i))*f(i)*N/(N+h))))); + h = sqrt(X^2+Y^2)/cos(phi)-N; + + iterations = iterations + 1; + if iterations > 100 + fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh); + break; + end end phi = phi*180/pi; @@ -57,4 +54,5 @@ lambda = lambda*180/pi; %fprintf('\n phi =%3.0f %3.0f %8.5f',b(1),b(2),b(3)) %fprintf('\n lambda =%3.0f %3.0f %8.5f',l(1),l(2),l(3)) %fprintf('\n h =%14.3f\n',h) + %%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/cart2utm.m b/src/utils/matlab/libs/geoFunctions/cart2utm.m index b3bec8969..8617bf541 100644 --- a/src/utils/matlab/libs/geoFunctions/cart2utm.m +++ b/src/utils/matlab/libs/geoFunctions/cart2utm.m @@ -1,7 +1,7 @@ function [E, N, U] = cart2utm(X, Y, Z, zone) -%CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'. +% CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'. % -%[E, N, U] = cart2utm(X, Y, Z, zone); +% [E, N, U] = cart2utm(X, Y, Z, zone); % % Inputs: % X,Y,Z - Cartesian coordinates. Coordinates are referenced @@ -12,19 +12,16 @@ function [E, N, U] = cart2utm(X, Y, Z, zone) % Outputs: % E, N, U - UTM coordinates (Easting, Northing, Uping) -%Kai Borre -11-1994 -%Copyright (c) by Kai Borre -% -% CVS record: -% $Id: cart2utm.m,v 1.1.1.1.2.6 2007/01/30 09:45:12 dpl Exp $ +% Kai Borre -11-1994 +% Copyright (c) by Kai Borre -%This implementation is based upon -%O. Andersson & K. Poder (1981) Koordinattransformationer +% This implementation is based upon +% O. Andersson & K. Poder (1981) Koordinattransformationer % ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren % Vol. 30: 552--571 and Vol. 31: 76 % -%An excellent, general reference (KW) is -%R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der +% An excellent, general reference (KW) is +% R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der % h\"oheren Geod\"asie und Kartographie. % Erster Band, Springer Verlag @@ -52,8 +49,8 @@ c = a * sqrt(1+ex2); vec = [X; Y; Z-4.5]; alpha = .756e-6; R = [ 1 -alpha 0; - alpha 1 0; - 0 0 1]; + alpha 1 0; + 0 0 1]; trans = [89.5; 93.8; 127.6]; scale = 0.9999988; v = scale*R*vec + trans; % coordinate vector in ED50 @@ -68,78 +65,78 @@ while abs(U-oldU) > 1.e-4 N1 = c/sqrt(1+ex2*(cos(B))^2); B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) ); U = norm(v(1:2))/cos(B)-N1; - - iterations = iterations + 1; - if iterations > 100 - fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU); - break; - end + + iterations = iterations + 1; + if iterations > 100 + fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU); + break; + end end -%Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) +% Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) m0 = 0.0004; n = f / (2-f); m = n^2 * (1/4 + n*n/64); w = (a*(-n-m0+m*(1-m0))) / (1+n); Q_n = a + w; -%Easting and longitude of central meridian +% Easting and longitude of central meridian E0 = 500000; L0 = (zone-30)*6 - 3; -%Check tolerance for reverse transformation +% Check tolerance for reverse transformation tolutm = pi/2 * 1.2e-10 * Q_n; tolgeo = 0.000040; -%Coefficients of trigonometric series +% Coefficients of trigonometric series -%ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52) +% ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52) % bg[1] = n*(-2 + n*(2/3 + n*(4/3 + n*(-82/45)))); % bg[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9))); % bg[3] = n^3*(-26/15 + n*34/21); % bg[4] = n^4*1237/630; -%spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62) +% spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62) % gb[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45))); % gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45))); % gb[3] = n^3*(56/15 + n*(-136/35)); % gb[4] = n^4*4279/630; -%spherical to ellipsoidal N, E, KW p. 196, (69) -% gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180))); -% gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440)); -% gtu[3] = n^3*(61/240 + n*(-103/140)); -% gtu[4] = n^4*49561/161280; +% spherical to ellipsoidal N, E, KW p. 196, (69) +% gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180))); +% gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440)); +% gtu[3] = n^3*(61/240 + n*(-103/140)); +% gtu[4] = n^4*49561/161280; -%ellipsoidal to spherical N, E, KW p. 194, (65) -% utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360))); -% utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440)); -% utg[3] = n^3*(-17/480 + n*37/840); -% utg[4] = n^4*(-4397/161280); +% ellipsoidal to spherical N, E, KW p. 194, (65) +% utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360))); +% utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440)); +% utg[3] = n^3*(-17/480 + n*37/840); +% utg[4] = n^4*(-4397/161280); -%With f = 1/297 we get +% With f = 1/297 we get bg = [-3.37077907e-3; - 4.73444769e-6; - -8.29914570e-9; - 1.58785330e-11]; + 4.73444769e-6; + -8.29914570e-9; + 1.58785330e-11]; gb = [ 3.37077588e-3; - 6.62769080e-6; - 1.78718601e-8; - 5.49266312e-11]; + 6.62769080e-6; + 1.78718601e-8; + 5.49266312e-11]; gtu = [ 8.41275991e-4; - 7.67306686e-7; - 1.21291230e-9; - 2.48508228e-12]; + 7.67306686e-7; + 1.21291230e-9; + 2.48508228e-12]; utg = [-8.41276339e-4; - -5.95619298e-8; - -1.69485209e-10; - -2.20473896e-13]; + -5.95619298e-8; + -1.69485209e-10; + -2.20473896e-13]; -%Ellipsoidal latitude, longitude to spherical latitude, longitude +% Ellipsoidal latitude, longitude to spherical latitude, longitude neg_geo = 'FALSE'; if B < 0 @@ -152,7 +149,7 @@ Bg_r = Bg_r + res_clensin; L0 = L0*pi / 180; Lg_r = L - L0; -%Spherical latitude, longitude to complementary spherical latitude +% Spherical latitude, longitude to complementary spherical latitude % i.e. spherical N, E cos_BN = cos(Bg_r); Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN); diff --git a/src/utils/matlab/libs/geoFunctions/check_t.m b/src/utils/matlab/libs/geoFunctions/check_t.m index 9d503c3e9..5c3cb6148 100644 --- a/src/utils/matlab/libs/geoFunctions/check_t.m +++ b/src/utils/matlab/libs/geoFunctions/check_t.m @@ -1,7 +1,7 @@ function corrTime = check_t(time) -%CHECK_T accounting for beginning or end of week crossover. +% CHECK_T accounting for beginning or end of week crossover. % -%corrTime = check_t(time); +% corrTime = check_t(time); % % Inputs: % time - time in seconds @@ -9,11 +9,8 @@ function corrTime = check_t(time) % Outputs: % corrTime - corrected time (seconds) -%Kai Borre 04-01-96 -%Copyright (c) by Kai Borre -% -% CVS record: -% $Id: check_t.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ +% Kai Borre 04-01-96 +% Copyright (c) by Kai Borre %========================================================================== half_week = 302400; % seconds @@ -24,5 +21,6 @@ if time > half_week corrTime = time - 2*half_week; elseif time < -half_week corrTime = time + 2*half_week; -end -%%%%%%% end check_t.m %%%%%%%%%%%%%%%%% \ No newline at end of file +end + +%%%%%%% end check_t.m %%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/clksin.m b/src/utils/matlab/libs/geoFunctions/clksin.m index 7ccd4726f..99a16331e 100644 --- a/src/utils/matlab/libs/geoFunctions/clksin.m +++ b/src/utils/matlab/libs/geoFunctions/clksin.m @@ -1,14 +1,12 @@ function [re, im] = clksin(ar, degree, arg_real, arg_imag) -%Clenshaw summation of sinus with complex argument -%[re, im] = clksin(ar, degree, arg_real, arg_imag); +% Clenshaw summation of sinus with complex argument +% [re, im] = clksin(ar, degree, arg_real, arg_imag); % Written by Kai Borre % December 20, 1995 % % See also WGS2UTM or CART2UTM % -% CVS record: -% $Id: clksin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ %========================================================================== sin_arg_r = sin(arg_real); diff --git a/src/utils/matlab/libs/geoFunctions/clsin.m b/src/utils/matlab/libs/geoFunctions/clsin.m index d499e8598..15b6d5b7b 100644 --- a/src/utils/matlab/libs/geoFunctions/clsin.m +++ b/src/utils/matlab/libs/geoFunctions/clsin.m @@ -1,15 +1,12 @@ function result = clsin(ar, degree, argument) -%Clenshaw summation of sinus of argument. +% Clenshaw summation of sinus of argument. % -%result = clsin(ar, degree, argument); +% result = clsin(ar, degree, argument); % Written by Kai Borre % December 20, 1995 % % See also WGS2UTM or CART2UTM -% -% CVS record: -% $Id: clsin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ %========================================================================== cos_arg = 2 * cos(argument); @@ -17,10 +14,11 @@ hr1 = 0; hr = 0; for t = degree : -1 : 1 - hr2 = hr1; - hr1 = hr; - hr = ar(t) + cos_arg*hr1 - hr2; + hr2 = hr1; + hr1 = hr; + hr = ar(t) + cos_arg*hr1 - hr2; end -result = hr * sin(argument); -%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%% \ No newline at end of file +result = hr * sin(argument); + +%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/deg2dms.m b/src/utils/matlab/libs/geoFunctions/deg2dms.m index 56ce6ae90..c7fc195df 100644 --- a/src/utils/matlab/libs/geoFunctions/deg2dms.m +++ b/src/utils/matlab/libs/geoFunctions/deg2dms.m @@ -1,6 +1,6 @@ function dmsOutput = deg2dms(deg) -%DEG2DMS Conversion of degrees to degrees, minutes, and seconds. -%The output format (dms format) is: (degrees*100 + minutes + seconds/100) +% DEG2DMS Conversion of degrees to degrees, minutes, and seconds. +% The output format (dms format) is: (degrees*100 + minutes + seconds/100) % Written by Kai Borre % February 7, 2001 @@ -11,7 +11,7 @@ neg_arg = false; if deg < 0 % Only positive numbers should be used while spliting into deg/min/sec deg = -deg; - neg_arg = true; + neg_arg = true; end %%% Split degrees minutes and seconds @@ -40,4 +40,4 @@ if neg_arg == true dmsOutput = -dmsOutput; end -%%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%% \ No newline at end of file +%%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/dms2deg.m b/src/utils/matlab/libs/geoFunctions/dms2deg.m index 077fc8639..1b4420dd0 100644 --- a/src/utils/matlab/libs/geoFunctions/dms2deg.m +++ b/src/utils/matlab/libs/geoFunctions/dms2deg.m @@ -1,12 +1,12 @@ function deg = dms2deg(dms) -%DMS2DEG Conversion of degrees, minutes, and seconds to degrees. +% DMS2DEG Conversion of degrees, minutes, and seconds to degrees. % Written by Javier Arribas 2011 % December 7, 2011 %if (dms(1)>=0) - deg=dms(1)+dms(2)/60+dms(3)/3600; +deg=dms(1)+dms(2)/60+dms(3)/3600; %else - %deg=dms(1)-dms(2)/60-dms(3)/3600; +%deg=dms(1)-dms(2)/60-dms(3)/3600; %end diff --git a/src/utils/matlab/libs/geoFunctions/dms2mat.m b/src/utils/matlab/libs/geoFunctions/dms2mat.m index da17b590b..a7d5b7023 100644 --- a/src/utils/matlab/libs/geoFunctions/dms2mat.m +++ b/src/utils/matlab/libs/geoFunctions/dms2mat.m @@ -1,6 +1,6 @@ function [dout,mout,sout] = dms2mat(dms,n) -%DMS2MAT Converts a dms vector format to a [deg min sec] matrix +% DMS2MAT Converts a dms vector format to a [deg min sec] matrix % % [d,m,s] = DMS2MAT(dms) converts a dms vector format to a % deg:min:sec matrix. The vector format is dms = 100*deg + min + sec/100. @@ -19,7 +19,7 @@ function [dout,mout,sout] = dms2mat(dms,n) % Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. % Written by: E. Byrns, E. Brown -% $Revision: 1.10 $ $Date: 2002/03/20 21:25:06 $ +% Revision: 1.10 $Date: 2002/03/20 21:25:06 if nargin == 0 @@ -71,7 +71,7 @@ if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx) - 60; end if any(m > 59) | any (m < 0) error('Minutes must be >= 0 and <= 59') - + elseif any(s >= 60) | any( s < 0) error('Seconds must be >= 0 and < 60') end diff --git a/src/utils/matlab/libs/geoFunctions/e_r_corr.m b/src/utils/matlab/libs/geoFunctions/e_r_corr.m index 974b233ba..4d93d9f2f 100644 --- a/src/utils/matlab/libs/geoFunctions/e_r_corr.m +++ b/src/utils/matlab/libs/geoFunctions/e_r_corr.m @@ -1,8 +1,8 @@ function X_sat_rot = e_r_corr(traveltime, X_sat) -%E_R_CORR Returns rotated satellite ECEF coordinates due to Earth -%rotation during signal travel time +% E_R_CORR Returns rotated satellite ECEF coordinates due to Earth +% rotation during signal travel time % -%X_sat_rot = e_r_corr(traveltime, X_sat); +% X_sat_rot = e_r_corr(traveltime, X_sat); % % Inputs: % travelTime - signal travel time @@ -11,11 +11,8 @@ function X_sat_rot = e_r_corr(traveltime, X_sat) % Outputs: % X_sat_rot - rotated satellite's coordinates (ECEF) -%Written by Kai Borre -%Copyright (c) by Kai Borre -% -% CVS record: -% $Id: e_r_corr.m,v 1.1.1.1.2.6 2006/08/22 13:45:59 dpl Exp $ +% Written by Kai Borre +% Copyright (c) by Kai Borre %========================================================================== Omegae_dot = 7.292115147e-5; % rad/sec @@ -25,10 +22,10 @@ omegatau = Omegae_dot * traveltime; %--- Make a rotation matrix ----------------------------------------------- R3 = [ cos(omegatau) sin(omegatau) 0; - -sin(omegatau) cos(omegatau) 0; - 0 0 1]; + -sin(omegatau) cos(omegatau) 0; + 0 0 1]; %--- Do the rotation ------------------------------------------------------ X_sat_rot = R3 * X_sat; -%%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%% \ No newline at end of file +%%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/findUtmZone.m b/src/utils/matlab/libs/geoFunctions/findUtmZone.m index 7630bf8e1..1294c7837 100644 --- a/src/utils/matlab/libs/geoFunctions/findUtmZone.m +++ b/src/utils/matlab/libs/geoFunctions/findUtmZone.m @@ -1,17 +1,17 @@ function utmZone = findUtmZone(latitude, longitude) -%Function finds the UTM zone number for given longitude and latitude. -%The longitude value must be between -180 (180 degree West) and 180 (180 -%degree East) degree. The latitude must be within -80 (80 degree South) and -%84 (84 degree North). +% Function finds the UTM zone number for given longitude and latitude. +% The longitude value must be between -180 (180 degree West) and 180 (180 +% degree East) degree. The latitude must be within -80 (80 degree South) and +% 84 (84 degree North). % -%utmZone = findUtmZone(latitude, longitude); +% utmZone = findUtmZone(latitude, longitude); % -%Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not -%15 deg 30 min). +% Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not +% 15 deg 30 min). %-------------------------------------------------------------------------- % SoftGNSS v3.0 -% +% % Copyright (C) Darius Plausinaitis % Written by Darius Plausinaitis %-------------------------------------------------------------------------- @@ -31,9 +31,6 @@ function utmZone = findUtmZone(latitude, longitude) %USA. %========================================================================== -%CVS record: -%$Id: findUtmZone.m,v 1.1.2.2 2006/08/22 13:45:59 dpl Exp $ - %% Check value bounds ===================================================== if ((longitude > 180) || (longitude < -180)) @@ -62,11 +59,11 @@ if (latitude > 72) utmZone = 35; elseif ((longitude >= 33) && (longitude < 42)) utmZone = 37; - end + end elseif ((latitude >= 56) && (latitude < 64)) % Correction for zone 32 if ((longitude >= 3) && (longitude < 12)) utmZone = 32; end -end \ No newline at end of file +end diff --git a/src/utils/matlab/libs/geoFunctions/geo2cart.m b/src/utils/matlab/libs/geoFunctions/geo2cart.m index 02f0d5768..3297e966a 100644 --- a/src/utils/matlab/libs/geoFunctions/geo2cart.m +++ b/src/utils/matlab/libs/geoFunctions/geo2cart.m @@ -1,13 +1,13 @@ function [X, Y, Z] = geo2cart(phi, lambda, h, i) -%GEO2CART Conversion of geographical coordinates (phi, lambda, h) to -%Cartesian coordinates (X, Y, Z). +% GEO2CART Conversion of geographical coordinates (phi, lambda, h) to +% Cartesian coordinates (X, Y, Z). % -%[X, Y, Z] = geo2cart(phi, lambda, h, i); +% [X, Y, Z] = geo2cart(phi, lambda, h, i); % -%Format for phi and lambda: [degrees minutes seconds]. -%h, X, Y, and Z are in meters. +% Format for phi and lambda: [degrees minutes seconds]. +% h, X, Y, and Z are in meters. % -%Choices i of Reference Ellipsoid +% Choices i of Reference Ellipsoid % 1. International Ellipsoid 1924 % 2. International Ellipsoid 1967 % 3. World Geodetic System 1972 @@ -16,18 +16,15 @@ function [X, Y, Z] = geo2cart(phi, lambda, h, i) % % Inputs: % phi - geocentric latitude (format [degrees minutes seconds]) -% lambda - geocentric longitude (format [degrees minutes seconds]) +% lambda - geocentric longitude (format [degrees minutes seconds]) % h - height % i - reference ellipsoid type % % Outputs: % X, Y, Z - Cartesian coordinates (meters) -%Kai Borre 10-13-98 -%Copyright (c) by Kai Borre -% -% CVS record: -% $Id: geo2cart.m,v 1.1.2.7 2006/08/22 13:45:59 dpl Exp $ +% Kai Borre 10-13-98 +% Copyright (c) by Kai Borre %========================================================================== b = phi(1) + phi(2)/60 + phi(3)/3600; @@ -44,5 +41,6 @@ N = c / sqrt(1 + ex2*cos(b)^2); X = (N+h) * cos(b) * cos(l); Y = (N+h) * cos(b) * sin(l); -Z = ((1-f(i))^2*N + h) * sin(b); +Z = ((1-f(i))^2*N + h) * sin(b); + %%%%%%%%%%%%%% end geo2cart.m %%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/leastSquarePos.m b/src/utils/matlab/libs/geoFunctions/leastSquarePos.m index 07d04f080..6f5c46f4f 100644 --- a/src/utils/matlab/libs/geoFunctions/leastSquarePos.m +++ b/src/utils/matlab/libs/geoFunctions/leastSquarePos.m @@ -1,7 +1,7 @@ function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings) -%Function calculates the Least Square Solution. +% Function calculates the Least Square Solution. % -%[pos, el, az, dop] = leastSquarePos(satpos, obs, settings); +% [pos, el, az, dop] = leastSquarePos(satpos, obs, settings); % % Inputs: % satpos - Satellites positions (in ECEF system: [X; Y; Z;] - @@ -12,8 +12,8 @@ function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings) % settings - receiver settings % % Outputs: -% pos - receiver position and receiver clock error -% (in ECEF system: [X, Y, Z, dt]) +% pos - receiver position and receiver clock error +% (in ECEF system: [X, Y, Z, dt]) % el - Satellites elevation angles (degrees) % az - Satellites azimuth angles (degrees) % dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) @@ -24,9 +24,6 @@ function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings) %Based on Kai Borre %Copyright (c) by Kai Borre %Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen -% -% CVS record: -% $Id: leastSquarePos.m,v 1.1.2.12 2006/08/22 13:45:59 dpl Exp $ %========================================================================== %=== Initialization ======================================================= @@ -44,7 +41,7 @@ el = az; %=== Iteratively find receiver position =================================== for iter = 1:nmbOfIterations - + for i = 1:nmbOfSatellites if iter == 1 %--- Initialize variables at the first iteration -------------- @@ -53,41 +50,41 @@ for iter = 1:nmbOfIterations else %--- Update equations ----------------------------------------- rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... - (X(3, i) - pos(3))^2; + (X(3, i) - pos(3))^2; traveltime = sqrt(rho2) / settings.c ; - + %--- Correct satellite position (do to earth rotation) -------- Rot_X = e_r_corr(traveltime, X(:, i)); - + %--- Find the elevation angel of the satellite ---------------- [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); - + if (settings.useTropCorr == 1) %--- Calculate tropospheric correction -------------------- trop = tropo(sin(el(i) * dtr), ... - 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); + 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); else % Do not calculate or apply the tropospheric corrections trop = 0; end - end % if iter == 1 ... ... else - + end % if iter == 1 ... ... else + %--- Apply the corrections ---------------------------------------- omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop); - + %--- Construct the A matrix --------------------------------------- A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... - (-(Rot_X(2) - pos(2))) / obs(i) ... - (-(Rot_X(3) - pos(3))) / obs(i) ... - 1 ]; + (-(Rot_X(2) - pos(2))) / obs(i) ... + (-(Rot_X(3) - pos(3))) / obs(i) ... + 1 ]; end % for i = 1:nmbOfSatellites - + % These lines allow the code to exit gracefully in case of any errors if rank(A) ~= 4 pos = zeros(1, 4); return end - + %--- Find position update --------------------------------------------- x = A \ omc; @@ -106,7 +103,7 @@ if nargout == 4 %--- Calculate DOP ---------------------------------------------------- Q = inv(A'*A); - dop(1) = sqrt(trace(Q)); % GDOP + dop(1) = sqrt(trace(Q)); % GDOP dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP dop(4) = sqrt(Q(3,3)); % VDOP diff --git a/src/utils/matlab/libs/geoFunctions/mat2dms.m b/src/utils/matlab/libs/geoFunctions/mat2dms.m index 839a0a62a..1c3787ab6 100644 --- a/src/utils/matlab/libs/geoFunctions/mat2dms.m +++ b/src/utils/matlab/libs/geoFunctions/mat2dms.m @@ -1,6 +1,5 @@ function dmsvec = mat2dms(d,m,s,n) - -%MAT2DMS Converts a [deg min sec] matrix to vector format +% MAT2DMS Converts a [deg min sec] matrix to vector format % % dms = MAT2DMS(d,m,s) converts a deg:min:sec matrix into a vector % format. The vector format is dms = 100*deg + min + sec/100. @@ -24,12 +23,12 @@ function dmsvec = mat2dms(d,m,s,n) % Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. % Written by: E. Byrns, E. Brown -% $Revision: 1.10 $ $Date: 2002/03/20 21:25:51 $ +% Revision: 1.10 Date: 2002/03/20 21:25:51 if nargin == 0 error('Incorrect number of arguments') - + elseif nargin==1 if size(d,2)== 3 s = d(:,3); m = d(:,2); d = d(:,1); @@ -41,11 +40,11 @@ elseif nargin==1 error('Single input matrices must be n-by-2 or n-by-3.'); end n = -5; - + elseif nargin == 2 s = zeros(size(d)); n = -5; - + elseif nargin == 3 n = -5; end diff --git a/src/utils/matlab/libs/geoFunctions/roundn.m b/src/utils/matlab/libs/geoFunctions/roundn.m index 936e114a5..ef9c53f4c 100644 --- a/src/utils/matlab/libs/geoFunctions/roundn.m +++ b/src/utils/matlab/libs/geoFunctions/roundn.m @@ -1,6 +1,6 @@ function [x,msg] = roundn(x,n) -%ROUNDN Rounds input data at specified power of 10 +% ROUNDN Rounds input data at specified power of 10 % % y = ROUNDN(x) rounds the input data x to the nearest hundredth. % @@ -15,7 +15,7 @@ function [x,msg] = roundn(x,n) % Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc. % Written by: E. Byrns, E. Brown -% $Revision: 1.9 $ $Date: 2002/03/20 21:26:19 $ +% Revision: 1.9 Date: 2002/03/20 21:26:19 msg = []; % Initialize output @@ -43,4 +43,4 @@ factors = 10 ^ (fix(-n)); % Set the significant digits for the input data -x = round(x * factors) / factors; \ No newline at end of file +x = round(x * factors) / factors; diff --git a/src/utils/matlab/libs/geoFunctions/satpos.m b/src/utils/matlab/libs/geoFunctions/satpos.m index 14adf6570..11f4f101e 100644 --- a/src/utils/matlab/libs/geoFunctions/satpos.m +++ b/src/utils/matlab/libs/geoFunctions/satpos.m @@ -1,9 +1,9 @@ function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ... - eph, settings) -%SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for -%given ephemeris EPH. Coordinates are computed for each satellite in the -%list PRNLIST. -%[satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings); + eph, settings) +% SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for +% given ephemeris EPH. Coordinates are computed for each satellite in the +% list PRNLIST. +%[ satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings); % % Inputs: % transmitTime - transmission time @@ -18,25 +18,22 @@ function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ... %-------------------------------------------------------------------------- % SoftGNSS v3.0 %-------------------------------------------------------------------------- -%Based on Kai Borre 04-09-96 -%Copyright (c) by Kai Borre -%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen -% -% CVS record: -% $Id: satpos.m,v 1.1.2.17 2007/01/30 09:45:12 dpl Exp $ +% Based on Kai Borre 04-09-96 +% Copyright (c) by Kai Borre +% Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen %% Initialize constants =================================================== numOfSatellites = size(prnList, 2); % GPS constatns -gpsPi = 3.1415926535898; % Pi used in the GPS coordinate - % system +gpsPi = 3.1415926535898; % Pi used in the GPS coordinate +% system %--- Constants for satellite position calculation ------------------------- Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s] GM = 3.986005e14; % Universal gravitational constant times - % the mass of the Earth, [m^3/s^2] +% the mass of the Earth, [m^3/s^2] F = -4.442807633e-10; % Constant, [sec/(meter)^(1/2)] %% Initialize results ===================================================== @@ -49,65 +46,65 @@ for satNr = 1 : numOfSatellites prn = prnList(satNr); -%% Find initial satellite clock correction -------------------------------- - + %% Find initial satellite clock correction -------------------------------- + %--- Find time difference --------------------------------------------- dt = check_t(transmitTime - eph(prn).t_oc); - + %--- Calculate clock correction --------------------------------------- satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... - eph(prn).a_f0 - ... - eph(prn).T_GD; - + eph(prn).a_f0 - ... + eph(prn).T_GD; + time = transmitTime - satClkCorr(satNr); - -%% Find satellite's position ---------------------------------------------- - + + %% Find satellite's position ---------------------------------------------- + %Restore semi-major axis a = eph(prn).sqrtA * eph(prn).sqrtA; - + %Time correction tk = check_t(time - eph(prn).t_oe); - + %Initial mean motion n0 = sqrt(GM / a^3); %Mean motion n = n0 + eph(prn).deltan; - + %Mean anomaly M = eph(prn).M_0 + n * tk; %Reduce mean anomaly to between 0 and 360 deg M = rem(M + 2*gpsPi, 2*gpsPi); - + %Initial guess of eccentric anomaly E = M; - + %--- Iteratively compute eccentric anomaly ---------------------------- for ii = 1:10 E_old = E; E = M + eph(prn).e * sin(E); dE = rem(E - E_old, 2*gpsPi); - + if abs(dE) < 1.e-12 - % Necessary precision is reached, exit from the loop + % Necessary precision is reached, exit from the loop break; end - end - + end + %Reduce eccentric anomaly to between 0 and 360 deg E = rem(E + 2*gpsPi, 2*gpsPi); - + %Compute relativistic correction term dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E); - + %Calculate the true anomaly nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e); - + %Compute angle phi phi = nu + eph(prn).omega; %Reduce phi to between 0 and 360 deg phi = rem(phi, 2*gpsPi); - + %Correct argument of latitude u = phi + ... eph(prn).C_uc * cos(2*phi) + ... @@ -120,22 +117,22 @@ for satNr = 1 : numOfSatellites i = eph(prn).i_0 + eph(prn).iDot * tk + ... eph(prn).C_ic * cos(2*phi) + ... eph(prn).C_is * sin(2*phi); - + %Compute the angle between the ascending node and the Greenwich meridian Omega = eph(prn).omega_0 + (eph(prn).omegaDot - Omegae_dot)*tk - ... - Omegae_dot * eph(prn).t_oe; + Omegae_dot * eph(prn).t_oe; %Reduce to between 0 and 360 deg Omega = rem(Omega + 2*gpsPi, 2*gpsPi); - + %--- Compute satellite coordinates ------------------------------------ satPositions(1, satNr) = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega); satPositions(2, satNr) = cos(u)*r * sin(Omega) + sin(u)*r * cos(i)*cos(Omega); satPositions(3, satNr) = sin(u)*r * sin(i); - - -%% Include relativistic correction in clock correction -------------------- + + + %% Include relativistic correction in clock correction -------------------- satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... - eph(prn).a_f0 - ... - eph(prn).T_GD + dtr; - + eph(prn).a_f0 - ... + eph(prn).T_GD + dtr; + end % for satNr = 1 : numOfSatellites diff --git a/src/utils/matlab/libs/geoFunctions/togeod.m b/src/utils/matlab/libs/geoFunctions/togeod.m index 99a43cc2c..9f7812d41 100644 --- a/src/utils/matlab/libs/geoFunctions/togeod.m +++ b/src/utils/matlab/libs/geoFunctions/togeod.m @@ -1,9 +1,9 @@ function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z) -%TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, +% TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, % height given Cartesian coordinates X,Y,Z, and reference ellipsoid % values semi-major axis (a) and the inverse of flattening (finv). % -%[dphi, dlambda, h] = togeod(a, finv, X, Y, Z); +% [dphi, dlambda, h] = togeod(a, finv, X, Y, Z); % % The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc) % The output units of angular quantities will be in decimal degrees @@ -24,9 +24,6 @@ function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z) % Reprinted with permission of author, 1996 % Fortran code translated into MATLAB % Kai Borre 03-30-96 -% -% CVS record: -% $Id: togeod.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ %========================================================================== h = 0; @@ -100,7 +97,7 @@ for i = 1:maxit if (dP*dP + dZ*dZ < tolsq) break; end - + % Not Converged--Warn user if i == maxit fprintf([' Problem in TOGEOD, did not converge in %2.0f',... diff --git a/src/utils/matlab/libs/geoFunctions/topocent.m b/src/utils/matlab/libs/geoFunctions/topocent.m index a6f680bd1..7ec0aaede 100644 --- a/src/utils/matlab/libs/geoFunctions/topocent.m +++ b/src/utils/matlab/libs/geoFunctions/topocent.m @@ -1,24 +1,21 @@ function [Az, El, D] = topocent(X, dx) -%TOPOCENT Transformation of vector dx into topocentric coordinate +% TOPOCENT Transformation of vector dx into topocentric coordinate % system with origin at X. % Both parameters are 3 by 1 vectors. % -%[Az, El, D] = topocent(X, dx); +% [Az, El, D] = topocent(X, dx); % % Inputs: -% X - vector origin corrdinates (in ECEF system [X; Y; Z;]) -% dx - vector ([dX; dY; dZ;]). +% X - vector origin corrdinates (in ECEF system [X; Y; Z;]) +% dx - vector ([dX; dY; dZ;]). % % Outputs: % D - vector length. Units like units of the input % Az - azimuth from north positive clockwise, degrees % El - elevation angle, degrees -%Kai Borre 11-24-96 -%Copyright (c) by Kai Borre -% -% CVS record: -% $Id: topocent.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ +% Kai Borre 11-24-96 +% Copyright (c) by Kai Borre %========================================================================== dtr = pi/180; @@ -27,12 +24,12 @@ dtr = pi/180; cl = cos(lambda * dtr); sl = sin(lambda * dtr); -cb = cos(phi * dtr); +cb = cos(phi * dtr); sb = sin(phi * dtr); F = [-sl -sb*cl cb*cl; - cl -sb*sl cb*sl; - 0 cb sb]; + cl -sb*sl cb*sl; + 0 cb sb]; local_vector = F' * dx; E = local_vector(1); @@ -54,4 +51,4 @@ if Az < 0 end D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2); -%%%%%%%%% end topocent.m %%%%%%%%% \ No newline at end of file +%%%%%%%%% end topocent.m %%%%%%%%% diff --git a/src/utils/matlab/libs/geoFunctions/tropo.m b/src/utils/matlab/libs/geoFunctions/tropo.m index bd14b8c52..f0f08d218 100644 --- a/src/utils/matlab/libs/geoFunctions/tropo.m +++ b/src/utils/matlab/libs/geoFunctions/tropo.m @@ -1,9 +1,9 @@ function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum) -%TROPO Calculation of tropospheric correction. +% TROPO Calculation of tropospheric correction. % The range correction ddr in m is to be subtracted from % pseudo-ranges and carrier phases % -%ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum); +% ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum); % % Inputs: % sinel - sin of elevation angle of satellite @@ -26,9 +26,6 @@ function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum) % A Matlab reimplementation of a C code from driver. % Kai Borre 06-28-95 -% -% CVS record: -% $Id: tropo.m,v 1.1.1.1.2.4 2006/08/22 13:46:00 dpl Exp $ %========================================================================== a_e = 6378.137; % semi-major axis of earth ellipsoid @@ -60,14 +57,14 @@ while 1 % check to see if geometry is crazy if rtop < 0 - rtop = 0; - end + rtop = 0; + end rtop = sqrt(rtop) - (a_e+hsta)*sinel; a = -sinel/(htop-hsta); b = -b0*(1-sinel^2) / (htop-hsta); rn = zeros(8,1); - + for i = 1:8 rn(i) = rtop^(i+1); end @@ -77,17 +74,17 @@ while 1 b^2*(6*a^2+4*b)*1.428571e-1, 0, 0]; if b^2 > 1.0e-35 - alpha(7) = a*b^3/2; - alpha(8) = b^4/9; + alpha(7) = a*b^3/2; + alpha(8) = b^4/9; end - + dr = rtop; dr = dr + alpha*rn; tropo = tropo + dr*ref*1000; if done == 'TRUE ' - ddr = tropo; - break; + ddr = tropo; + break; end done = 'TRUE ';