mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 04:30:33 +00:00
Code Issues Releases Wiki Activity

Clean up Matlab/Octave code

Browse Source
This commit is contained in:
Carles Fernandez 2018-03-30 11:36:50 +02:00
parent c58107d56c
commit c9b2f06d41
18 changed files with 212 additions and 247 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/utils/matlab/libs/geoFunctions/cart2geo.m
View File

@ -1,8 +1,8 @@
function [phi, lambda, h] = cart2geo(X, Y, Z, i) function [phi, lambda, h] = cart2geo(X, Y, Z, i)
%CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical % CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical
%coordinates (phi, lambda, h) on a selected reference ellipsoid. % 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 % Choices i of Reference Ellipsoid for Geographical Coordinates
% 1. International Ellipsoid 1924 % 1. International Ellipsoid 1924
@ -11,12 +11,9 @@ function [phi, lambda, h] = cart2geo(X, Y, Z, i)
% 4. Geodetic Reference System 1980 % 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984 % 5. World Geodetic System 1984
%Kai Borre 10-13-98 % Kai Borre 10-13-98
%Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
%Revision: 1.0 Date: 1998/10/23 % 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 $
%========================================================================== %==========================================================================
a = [6378388 6378160 6378135 6378137 6378137]; 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; h = 0.1; oldh = 0;
iterations = 0; iterations = 0;
while abs(h-oldh) > 1.e-12 while abs(h-oldh) > 1.e-12
oldh = h; oldh = h;
N = c/sqrt(1+ex2*cos(phi)^2); 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))))); 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; h = sqrt(X^2+Y^2)/cos(phi)-N;
iterations = iterations + 1; iterations = iterations + 1;
if iterations > 100 if iterations > 100
fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh); fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh);
break; break;
end end
end end
phi = phi*180/pi; 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 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 lambda =%3.0f %3.0f %8.5f',l(1),l(2),l(3))
%fprintf('\n h =%14.3f\n',h) %fprintf('\n h =%14.3f\n',h)
%%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%%

97
src/utils/matlab/libs/geoFunctions/cart2utm.m
View File

@ -1,7 +1,7 @@
function [E, N, U] = cart2utm(X, Y, Z, zone) 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: % Inputs:
% X,Y,Z - Cartesian coordinates. Coordinates are referenced % X,Y,Z - Cartesian coordinates. Coordinates are referenced
@ -12,19 +12,16 @@ function [E, N, U] = cart2utm(X, Y, Z, zone)
% Outputs: % Outputs:
% E, N, U - UTM coordinates (Easting, Northing, Uping) % E, N, U - UTM coordinates (Easting, Northing, Uping)
%Kai Borre -11-1994 % Kai Borre -11-1994
%Copyright (c) by Kai Borre % 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 $
%This implementation is based upon % This implementation is based upon
%O. Andersson & K. Poder (1981) Koordinattransformationer % O. Andersson & K. Poder (1981) Koordinattransformationer
% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren % ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
% Vol. 30: 552--571 and Vol. 31: 76 % Vol. 30: 552--571 and Vol. 31: 76
% %
%An excellent, general reference (KW) is % An excellent, general reference (KW) is
%R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der % R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
% h\"oheren Geod\"asie und Kartographie. % h\"oheren Geod\"asie und Kartographie.
% Erster Band, Springer Verlag % Erster Band, Springer Verlag
@ -52,8 +49,8 @@ c = a * sqrt(1+ex2);
vec = [X; Y; Z-4.5]; vec = [X; Y; Z-4.5];
alpha = .756e-6; alpha = .756e-6;
R = [ 1 -alpha 0; R = [ 1 -alpha 0;
alpha 1 0; alpha 1 0;
0 0 1]; 0 0 1];
trans = [89.5; 93.8; 127.6]; trans = [89.5; 93.8; 127.6];
scale = 0.9999988; scale = 0.9999988;
v = scale*R*vec + trans; % coordinate vector in ED50 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); N1 = c/sqrt(1+ex2*(cos(B))^2);
B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) ); B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) );
U = norm(v(1:2))/cos(B)-N1; U = norm(v(1:2))/cos(B)-N1;
iterations = iterations + 1; iterations = iterations + 1;
if iterations > 100 if iterations > 100
fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU); fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU);
break; break;
end end
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; m0 = 0.0004;
n = f / (2-f); n = f / (2-f);
m = n^2 * (1/4 + n*n/64); m = n^2 * (1/4 + n*n/64);
w = (a*(-n-m0+m*(1-m0))) / (1+n); w = (a*(-n-m0+m*(1-m0))) / (1+n);
Q_n = a + w; Q_n = a + w;
%Easting and longitude of central meridian % Easting and longitude of central meridian
E0 = 500000; E0 = 500000;
L0 = (zone-30)*6 - 3; L0 = (zone-30)*6 - 3;
%Check tolerance for reverse transformation % Check tolerance for reverse transformation
tolutm = pi/2 * 1.2e-10 * Q_n; tolutm = pi/2 * 1.2e-10 * Q_n;
tolgeo = 0.000040; 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[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[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9)));
% bg[3] = n^3*(-26/15 + n*34/21); % bg[3] = n^3*(-26/15 + n*34/21);
% bg[4] = n^4*1237/630; % 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[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45)));
% gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45))); % gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45)));
% gb[3] = n^3*(56/15 + n*(-136/35)); % gb[3] = n^3*(56/15 + n*(-136/35));
% gb[4] = n^4*4279/630; % gb[4] = n^4*4279/630;
%spherical to ellipsoidal N, E, KW p. 196, (69) % spherical to ellipsoidal N, E, KW p. 196, (69)
% gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180))); % 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[2] = n^2*(13/48 + n*(-3/5 + n*557/1440));
% gtu[3] = n^3*(61/240 + n*(-103/140)); % gtu[3] = n^3*(61/240 + n*(-103/140));
% gtu[4] = n^4*49561/161280; % gtu[4] = n^4*49561/161280;
%ellipsoidal to spherical N, E, KW p. 194, (65) % ellipsoidal to spherical N, E, KW p. 194, (65)
% utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360))); % 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[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440));
% utg[3] = n^3*(-17/480 + n*37/840); % utg[3] = n^3*(-17/480 + n*37/840);
% utg[4] = n^4*(-4397/161280); % utg[4] = n^4*(-4397/161280);
%With f = 1/297 we get % With f = 1/297 we get
bg = [-3.37077907e-3; bg = [-3.37077907e-3;
4.73444769e-6; 4.73444769e-6;
-8.29914570e-9; -8.29914570e-9;
1.58785330e-11]; 1.58785330e-11];
gb = [ 3.37077588e-3; gb = [ 3.37077588e-3;
6.62769080e-6; 6.62769080e-6;
1.78718601e-8; 1.78718601e-8;
5.49266312e-11]; 5.49266312e-11];
gtu = [ 8.41275991e-4; gtu = [ 8.41275991e-4;
7.67306686e-7; 7.67306686e-7;
1.21291230e-9; 1.21291230e-9;
2.48508228e-12]; 2.48508228e-12];
utg = [-8.41276339e-4; utg = [-8.41276339e-4;
-5.95619298e-8; -5.95619298e-8;
-1.69485209e-10; -1.69485209e-10;
-2.20473896e-13]; -2.20473896e-13];
%Ellipsoidal latitude, longitude to spherical latitude, longitude % Ellipsoidal latitude, longitude to spherical latitude, longitude
neg_geo = 'FALSE'; neg_geo = 'FALSE';
if B < 0 if B < 0
@ -152,7 +149,7 @@ Bg_r = Bg_r + res_clensin;
L0 = L0*pi / 180; L0 = L0*pi / 180;
Lg_r = L - L0; Lg_r = L - L0;
%Spherical latitude, longitude to complementary spherical latitude % Spherical latitude, longitude to complementary spherical latitude
% i.e. spherical N, E % i.e. spherical N, E
cos_BN = cos(Bg_r); cos_BN = cos(Bg_r);
Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN); Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN);

16
src/utils/matlab/libs/geoFunctions/check_t.m
View File

@ -1,7 +1,7 @@
function corrTime = check_t(time) 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: % Inputs:
% time - time in seconds % time - time in seconds
@ -9,11 +9,8 @@ function corrTime = check_t(time)
% Outputs: % Outputs:
% corrTime - corrected time (seconds) % corrTime - corrected time (seconds)
%Kai Borre 04-01-96 % Kai Borre 04-01-96
%Copyright (c) by Kai Borre % 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 $
%========================================================================== %==========================================================================
half_week = 302400; % seconds half_week = 302400; % seconds
@ -24,5 +21,6 @@ if time > half_week
corrTime = time - 2*half_week; corrTime = time - 2*half_week;
elseif time < -half_week elseif time < -half_week
corrTime = time + 2*half_week; corrTime = time + 2*half_week;
end end
%%%%%%% end check_t.m %%%%%%%%%%%%%%%%%
%%%%%%% end check_t.m %%%%%%%%%%%%%%%%%

6
src/utils/matlab/libs/geoFunctions/clksin.m
View File

@ -1,14 +1,12 @@
function [re, im] = clksin(ar, degree, arg_real, arg_imag) function [re, im] = clksin(ar, degree, arg_real, arg_imag)
%Clenshaw summation of sinus with complex argument % Clenshaw summation of sinus with complex argument
%[re, im] = clksin(ar, degree, arg_real, arg_imag); % [re, im] = clksin(ar, degree, arg_real, arg_imag);
% Written by Kai Borre % Written by Kai Borre
% December 20, 1995 % December 20, 1995
% %
% See also WGS2UTM or CART2UTM % 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); sin_arg_r = sin(arg_real);

18
src/utils/matlab/libs/geoFunctions/clsin.m
View File

@ -1,15 +1,12 @@
function result = clsin(ar, degree, argument) 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 % Written by Kai Borre
% December 20, 1995 % December 20, 1995
% %
% See also WGS2UTM or CART2UTM % 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); cos_arg = 2 * cos(argument);
@ -17,10 +14,11 @@ hr1 = 0;
hr = 0; hr = 0;
for t = degree : -1 : 1 for t = degree : -1 : 1
hr2 = hr1; hr2 = hr1;
hr1 = hr; hr1 = hr;
hr = ar(t) + cos_arg*hr1 - hr2; hr = ar(t) + cos_arg*hr1 - hr2;
end end
result = hr * sin(argument); result = hr * sin(argument);
%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%%

8
src/utils/matlab/libs/geoFunctions/deg2dms.m
View File

@ -1,6 +1,6 @@
function dmsOutput = deg2dms(deg) function dmsOutput = deg2dms(deg)
%DEG2DMS Conversion of degrees to degrees, minutes, and seconds. % DEG2DMS Conversion of degrees to degrees, minutes, and seconds.
%The output format (dms format) is: (degrees*100 + minutes + seconds/100) % The output format (dms format) is: (degrees*100 + minutes + seconds/100)
% Written by Kai Borre % Written by Kai Borre
% February 7, 2001 % February 7, 2001
@ -11,7 +11,7 @@ neg_arg = false;
if deg < 0 if deg < 0
% Only positive numbers should be used while spliting into deg/min/sec % Only positive numbers should be used while spliting into deg/min/sec
deg = -deg; deg = -deg;
neg_arg = true; neg_arg = true;
end end
%%% Split degrees minutes and seconds %%% Split degrees minutes and seconds
@ -40,4 +40,4 @@ if neg_arg == true
dmsOutput = -dmsOutput; dmsOutput = -dmsOutput;
end end
%%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%%

6
src/utils/matlab/libs/geoFunctions/dms2deg.m
View File

@ -1,12 +1,12 @@
function deg = dms2deg(dms) 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 % Written by Javier Arribas 2011
% December 7, 2011 % December 7, 2011
%if (dms(1)>=0) %if (dms(1)>=0)
deg=dms(1)+dms(2)/60+dms(3)/3600; deg=dms(1)+dms(2)/60+dms(3)/3600;
%else %else
%deg=dms(1)-dms(2)/60-dms(3)/3600; %deg=dms(1)-dms(2)/60-dms(3)/3600;
%end %end

6
src/utils/matlab/libs/geoFunctions/dms2mat.m
View File

@ -1,6 +1,6 @@
function [dout,mout,sout] = dms2mat(dms,n) 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 % [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. % 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. % Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% Written by: E. Byrns, E. Brown % 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 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) if any(m > 59) | any (m < 0)
error('Minutes must be >= 0 and <= 59') error('Minutes must be >= 0 and <= 59')
elseif any(s >= 60) | any( s < 0) elseif any(s >= 60) | any( s < 0)
error('Seconds must be >= 0 and < 60') error('Seconds must be >= 0 and < 60')
end end

19
src/utils/matlab/libs/geoFunctions/e_r_corr.m
View File

@ -1,8 +1,8 @@
function X_sat_rot = e_r_corr(traveltime, X_sat) function X_sat_rot = e_r_corr(traveltime, X_sat)
%E_R_CORR Returns rotated satellite ECEF coordinates due to Earth % E_R_CORR Returns rotated satellite ECEF coordinates due to Earth
%rotation during signal travel time % rotation during signal travel time
% %
%X_sat_rot = e_r_corr(traveltime, X_sat); % X_sat_rot = e_r_corr(traveltime, X_sat);
% %
% Inputs: % Inputs:
% travelTime - signal travel time % travelTime - signal travel time
@ -11,11 +11,8 @@ function X_sat_rot = e_r_corr(traveltime, X_sat)
% Outputs: % Outputs:
% X_sat_rot - rotated satellite's coordinates (ECEF) % X_sat_rot - rotated satellite's coordinates (ECEF)
%Written by Kai Borre % Written by Kai Borre
%Copyright (c) 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 $
%========================================================================== %==========================================================================
Omegae_dot = 7.292115147e-5; % rad/sec Omegae_dot = 7.292115147e-5; % rad/sec
@ -25,10 +22,10 @@ omegatau = Omegae_dot * traveltime;
%--- Make a rotation matrix ----------------------------------------------- %--- Make a rotation matrix -----------------------------------------------
R3 = [ cos(omegatau) sin(omegatau) 0; R3 = [ cos(omegatau) sin(omegatau) 0;
-sin(omegatau) cos(omegatau) 0; -sin(omegatau) cos(omegatau) 0;
0 0 1]; 0 0 1];
%--- Do the rotation ------------------------------------------------------ %--- Do the rotation ------------------------------------------------------
X_sat_rot = R3 * X_sat; X_sat_rot = R3 * X_sat;
%%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%% %%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%%

23
src/utils/matlab/libs/geoFunctions/findUtmZone.m
View File

@ -1,17 +1,17 @@
function utmZone = findUtmZone(latitude, longitude) function utmZone = findUtmZone(latitude, longitude)
%Function finds the UTM zone number for given longitude and latitude. % Function finds the UTM zone number for given longitude and latitude.
%The longitude value must be between -180 (180 degree West) and 180 (180 % 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 % degree East) degree. The latitude must be within -80 (80 degree South) and
%84 (84 degree North). % 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 % Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
%15 deg 30 min). % 15 deg 30 min).
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% SoftGNSS v3.0 % SoftGNSS v3.0
% %
% Copyright (C) Darius Plausinaitis % Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis % Written by Darius Plausinaitis
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
@ -31,9 +31,6 @@ function utmZone = findUtmZone(latitude, longitude)
%USA. %USA.
%========================================================================== %==========================================================================
%CVS record:
%$Id: findUtmZone.m,v 1.1.2.2 2006/08/22 13:45:59 dpl Exp $
%% Check value bounds ===================================================== %% Check value bounds =====================================================
if ((longitude > 180) || (longitude < -180)) if ((longitude > 180) || (longitude < -180))
@ -62,11 +59,11 @@ if (latitude > 72)
utmZone = 35; utmZone = 35;
elseif ((longitude >= 33) && (longitude < 42)) elseif ((longitude >= 33) && (longitude < 42))
utmZone = 37; utmZone = 37;
end end
elseif ((latitude >= 56) && (latitude < 64)) elseif ((latitude >= 56) && (latitude < 64))
% Correction for zone 32 % Correction for zone 32
if ((longitude >= 3) && (longitude < 12)) if ((longitude >= 3) && (longitude < 12))
utmZone = 32; utmZone = 32;
end end
end end

24
src/utils/matlab/libs/geoFunctions/geo2cart.m
View File

@ -1,13 +1,13 @@
function [X, Y, Z] = geo2cart(phi, lambda, h, i) function [X, Y, Z] = geo2cart(phi, lambda, h, i)
%GEO2CART Conversion of geographical coordinates (phi, lambda, h) to % GEO2CART Conversion of geographical coordinates (phi, lambda, h) to
%Cartesian coordinates (X, Y, Z). % 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]. % Format for phi and lambda: [degrees minutes seconds].
%h, X, Y, and Z are in meters. % h, X, Y, and Z are in meters.
% %
%Choices i of Reference Ellipsoid % Choices i of Reference Ellipsoid
% 1. International Ellipsoid 1924 % 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967 % 2. International Ellipsoid 1967
% 3. World Geodetic System 1972 % 3. World Geodetic System 1972
@ -16,18 +16,15 @@ function [X, Y, Z] = geo2cart(phi, lambda, h, i)
% %
% Inputs: % Inputs:
% phi - geocentric latitude (format [degrees minutes seconds]) % phi - geocentric latitude (format [degrees minutes seconds])
% lambda - geocentric longitude (format [degrees minutes seconds]) % lambda - geocentric longitude (format [degrees minutes seconds])
% h - height % h - height
% i - reference ellipsoid type % i - reference ellipsoid type
% %
% Outputs: % Outputs:
% X, Y, Z - Cartesian coordinates (meters) % X, Y, Z - Cartesian coordinates (meters)
%Kai Borre 10-13-98 % Kai Borre 10-13-98
%Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
%
% CVS record:
% $Id: geo2cart.m,v 1.1.2.7 2006/08/22 13:45:59 dpl Exp $
%========================================================================== %==========================================================================
b = phi(1) + phi(2)/60 + phi(3)/3600; 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); X = (N+h) * cos(b) * cos(l);
Y = (N+h) * cos(b) * sin(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 %%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%% end geo2cart.m %%%%%%%%%%%%%%%%%%%%%%%%

41
src/utils/matlab/libs/geoFunctions/leastSquarePos.m
View File

@ -1,7 +1,7 @@
function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings) 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: % Inputs:
% satpos - Satellites positions (in ECEF system: [X; Y; Z;] - % 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 % settings - receiver settings
% %
% Outputs: % Outputs:
% pos - receiver position and receiver clock error % pos - receiver position and receiver clock error
% (in ECEF system: [X, Y, Z, dt]) % (in ECEF system: [X, Y, Z, dt])
% el - Satellites elevation angles (degrees) % el - Satellites elevation angles (degrees)
% az - Satellites azimuth angles (degrees) % az - Satellites azimuth angles (degrees)
% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) % 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 %Based on Kai Borre
%Copyright (c) by Kai Borre %Copyright (c) by Kai Borre
%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen %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 ======================================================= %=== Initialization =======================================================
@ -44,7 +41,7 @@ el = az;
%=== Iteratively find receiver position =================================== %=== Iteratively find receiver position ===================================
for iter = 1:nmbOfIterations for iter = 1:nmbOfIterations
for i = 1:nmbOfSatellites for i = 1:nmbOfSatellites
if iter == 1 if iter == 1
%--- Initialize variables at the first iteration -------------- %--- Initialize variables at the first iteration --------------
@ -53,41 +50,41 @@ for iter = 1:nmbOfIterations
else else
%--- Update equations ----------------------------------------- %--- Update equations -----------------------------------------
rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... 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 ; traveltime = sqrt(rho2) / settings.c ;
%--- Correct satellite position (do to earth rotation) -------- %--- Correct satellite position (do to earth rotation) --------
Rot_X = e_r_corr(traveltime, X(:, i)); Rot_X = e_r_corr(traveltime, X(:, i));
%--- Find the elevation angel of the satellite ---------------- %--- Find the elevation angel of the satellite ----------------
[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
if (settings.useTropCorr == 1) if (settings.useTropCorr == 1)
%--- Calculate tropospheric correction -------------------- %--- Calculate tropospheric correction --------------------
trop = tropo(sin(el(i) * dtr), ... 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 else
% Do not calculate or apply the tropospheric corrections % Do not calculate or apply the tropospheric corrections
trop = 0; trop = 0;
end end
end % if iter == 1 ... ... else end % if iter == 1 ... ... else
%--- Apply the corrections ---------------------------------------- %--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop); omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop);
%--- Construct the A matrix --------------------------------------- %--- Construct the A matrix ---------------------------------------
A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ...
(-(Rot_X(2) - pos(2))) / obs(i) ... (-(Rot_X(2) - pos(2))) / obs(i) ...
(-(Rot_X(3) - pos(3))) / obs(i) ... (-(Rot_X(3) - pos(3))) / obs(i) ...
1 ]; 1 ];
end % for i = 1:nmbOfSatellites end % for i = 1:nmbOfSatellites
% These lines allow the code to exit gracefully in case of any errors % These lines allow the code to exit gracefully in case of any errors
if rank(A) ~= 4 if rank(A) ~= 4
pos = zeros(1, 4); pos = zeros(1, 4);
return return
end end
%--- Find position update --------------------------------------------- %--- Find position update ---------------------------------------------
x = A \ omc; x = A \ omc;
@ -106,7 +103,7 @@ if nargout == 4
%--- Calculate DOP ---------------------------------------------------- %--- Calculate DOP ----------------------------------------------------
Q = inv(A'*A); 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(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP
dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP
dop(4) = sqrt(Q(3,3)); % VDOP dop(4) = sqrt(Q(3,3)); % VDOP

11
src/utils/matlab/libs/geoFunctions/mat2dms.m
View File

@ -1,6 +1,5 @@
function dmsvec = mat2dms(d,m,s,n) 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 % 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. % 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. % Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% Written by: E. Byrns, E. Brown % 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 if nargin == 0
error('Incorrect number of arguments') error('Incorrect number of arguments')
elseif nargin==1 elseif nargin==1
if size(d,2)== 3 if size(d,2)== 3
s = d(:,3); m = d(:,2); d = d(:,1); 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.'); error('Single input matrices must be n-by-2 or n-by-3.');
end end
n = -5; n = -5;
elseif nargin == 2 elseif nargin == 2
s = zeros(size(d)); s = zeros(size(d));
n = -5; n = -5;
elseif nargin == 3 elseif nargin == 3
n = -5; n = -5;
end end

6
src/utils/matlab/libs/geoFunctions/roundn.m
View File

@ -1,6 +1,6 @@
function [x,msg] = roundn(x,n) 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. % 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. % Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% Written by: E. Byrns, E. Brown % 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 msg = []; % Initialize output
@ -43,4 +43,4 @@ factors = 10 ^ (fix(-n));
% Set the significant digits for the input data % Set the significant digits for the input data
x = round(x * factors) / factors; x = round(x * factors) / factors;

87
src/utils/matlab/libs/geoFunctions/satpos.m
View File

@ -1,9 +1,9 @@
function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ... function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ...
eph, settings) eph, settings)
%SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for % SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for
%given ephemeris EPH. Coordinates are computed for each satellite in the % given ephemeris EPH. Coordinates are computed for each satellite in the
%list PRNLIST. % list PRNLIST.
%[satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings); %[ satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings);
% %
% Inputs: % Inputs:
% transmitTime - transmission time % transmitTime - transmission time
@ -18,25 +18,22 @@ function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ...
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
% SoftGNSS v3.0 % SoftGNSS v3.0
%-------------------------------------------------------------------------- %--------------------------------------------------------------------------
%Based on Kai Borre 04-09-96 % Based on Kai Borre 04-09-96
%Copyright (c) by Kai Borre % Copyright (c) by Kai Borre
%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen % 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 $
%% Initialize constants =================================================== %% Initialize constants ===================================================
numOfSatellites = size(prnList, 2); numOfSatellites = size(prnList, 2);
% GPS constatns % GPS constatns
gpsPi = 3.1415926535898; % Pi used in the GPS coordinate gpsPi = 3.1415926535898; % Pi used in the GPS coordinate
% system % system
%--- Constants for satellite position calculation ------------------------- %--- Constants for satellite position calculation -------------------------
Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s] Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s]
GM = 3.986005e14; % Universal gravitational constant times 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)] F = -4.442807633e-10; % Constant, [sec/(meter)^(1/2)]
%% Initialize results ===================================================== %% Initialize results =====================================================
@ -49,65 +46,65 @@ for satNr = 1 : numOfSatellites
prn = prnList(satNr); prn = prnList(satNr);
%% Find initial satellite clock correction -------------------------------- %% Find initial satellite clock correction --------------------------------
%--- Find time difference --------------------------------------------- %--- Find time difference ---------------------------------------------
dt = check_t(transmitTime - eph(prn).t_oc); dt = check_t(transmitTime - eph(prn).t_oc);
%--- Calculate clock correction --------------------------------------- %--- Calculate clock correction ---------------------------------------
satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ...
eph(prn).a_f0 - ... eph(prn).a_f0 - ...
eph(prn).T_GD; eph(prn).T_GD;
time = transmitTime - satClkCorr(satNr); time = transmitTime - satClkCorr(satNr);
%% Find satellite's position ---------------------------------------------- %% Find satellite's position ----------------------------------------------
%Restore semi-major axis %Restore semi-major axis
a = eph(prn).sqrtA * eph(prn).sqrtA; a = eph(prn).sqrtA * eph(prn).sqrtA;
%Time correction %Time correction
tk = check_t(time - eph(prn).t_oe); tk = check_t(time - eph(prn).t_oe);
%Initial mean motion %Initial mean motion
n0 = sqrt(GM / a^3); n0 = sqrt(GM / a^3);
%Mean motion %Mean motion
n = n0 + eph(prn).deltan; n = n0 + eph(prn).deltan;
%Mean anomaly %Mean anomaly
M = eph(prn).M_0 + n * tk; M = eph(prn).M_0 + n * tk;
%Reduce mean anomaly to between 0 and 360 deg %Reduce mean anomaly to between 0 and 360 deg
M = rem(M + 2*gpsPi, 2*gpsPi); M = rem(M + 2*gpsPi, 2*gpsPi);
%Initial guess of eccentric anomaly %Initial guess of eccentric anomaly
E = M; E = M;
%--- Iteratively compute eccentric anomaly ---------------------------- %--- Iteratively compute eccentric anomaly ----------------------------
for ii = 1:10 for ii = 1:10
E_old = E; E_old = E;
E = M + eph(prn).e * sin(E); E = M + eph(prn).e * sin(E);
dE = rem(E - E_old, 2*gpsPi); dE = rem(E - E_old, 2*gpsPi);
if abs(dE) < 1.e-12 if abs(dE) < 1.e-12
% Necessary precision is reached, exit from the loop % Necessary precision is reached, exit from the loop
break; break;
end end
end end
%Reduce eccentric anomaly to between 0 and 360 deg %Reduce eccentric anomaly to between 0 and 360 deg
E = rem(E + 2*gpsPi, 2*gpsPi); E = rem(E + 2*gpsPi, 2*gpsPi);
%Compute relativistic correction term %Compute relativistic correction term
dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E); dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E);
%Calculate the true anomaly %Calculate the true anomaly
nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e); nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e);
%Compute angle phi %Compute angle phi
phi = nu + eph(prn).omega; phi = nu + eph(prn).omega;
%Reduce phi to between 0 and 360 deg %Reduce phi to between 0 and 360 deg
phi = rem(phi, 2*gpsPi); phi = rem(phi, 2*gpsPi);
%Correct argument of latitude %Correct argument of latitude
u = phi + ... u = phi + ...
eph(prn).C_uc * cos(2*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 + ... i = eph(prn).i_0 + eph(prn).iDot * tk + ...
eph(prn).C_ic * cos(2*phi) + ... eph(prn).C_ic * cos(2*phi) + ...
eph(prn).C_is * sin(2*phi); eph(prn).C_is * sin(2*phi);
%Compute the angle between the ascending node and the Greenwich meridian %Compute the angle between the ascending node and the Greenwich meridian
Omega = eph(prn).omega_0 + (eph(prn).omegaDot - Omegae_dot)*tk - ... 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 %Reduce to between 0 and 360 deg
Omega = rem(Omega + 2*gpsPi, 2*gpsPi); Omega = rem(Omega + 2*gpsPi, 2*gpsPi);
%--- Compute satellite coordinates ------------------------------------ %--- Compute satellite coordinates ------------------------------------
satPositions(1, satNr) = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega); 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(2, satNr) = cos(u)*r * sin(Omega) + sin(u)*r * cos(i)*cos(Omega);
satPositions(3, satNr) = sin(u)*r * sin(i); 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 + ... satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ...
eph(prn).a_f0 - ... eph(prn).a_f0 - ...
eph(prn).T_GD + dtr; eph(prn).T_GD + dtr;
end % for satNr = 1 : numOfSatellites end % for satNr = 1 : numOfSatellites

9
src/utils/matlab/libs/geoFunctions/togeod.m
View File

@ -1,9 +1,9 @@
function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z) 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 % height given Cartesian coordinates X,Y,Z, and reference ellipsoid
% values semi-major axis (a) and the inverse of flattening (finv). % 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 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 % 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 % Reprinted with permission of author, 1996
% Fortran code translated into MATLAB % Fortran code translated into MATLAB
% Kai Borre 03-30-96 % 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; h = 0;
@ -100,7 +97,7 @@ for i = 1:maxit
if (dP*dP + dZ*dZ < tolsq) if (dP*dP + dZ*dZ < tolsq)
break; break;
end end
% Not Converged--Warn user % Not Converged--Warn user
if i == maxit if i == maxit
fprintf([' Problem in TOGEOD, did not converge in %2.0f',... fprintf([' Problem in TOGEOD, did not converge in %2.0f',...

23
src/utils/matlab/libs/geoFunctions/topocent.m
View File

@ -1,24 +1,21 @@
function [Az, El, D] = topocent(X, dx) 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. % system with origin at X.
% Both parameters are 3 by 1 vectors. % Both parameters are 3 by 1 vectors.
% %
%[Az, El, D] = topocent(X, dx); % [Az, El, D] = topocent(X, dx);
% %
% Inputs: % Inputs:
% X - vector origin corrdinates (in ECEF system [X; Y; Z;]) % X - vector origin corrdinates (in ECEF system [X; Y; Z;])
% dx - vector ([dX; dY; dZ;]). % dx - vector ([dX; dY; dZ;]).
% %
% Outputs: % Outputs:
% D - vector length. Units like units of the input % D - vector length. Units like units of the input
% Az - azimuth from north positive clockwise, degrees % Az - azimuth from north positive clockwise, degrees
% El - elevation angle, degrees % El - elevation angle, degrees
%Kai Borre 11-24-96 % Kai Borre 11-24-96
%Copyright (c) by Kai Borre % 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 $
%========================================================================== %==========================================================================
dtr = pi/180; dtr = pi/180;
@ -27,12 +24,12 @@ dtr = pi/180;
cl = cos(lambda * dtr); cl = cos(lambda * dtr);
sl = sin(lambda * dtr); sl = sin(lambda * dtr);
cb = cos(phi * dtr); cb = cos(phi * dtr);
sb = sin(phi * dtr); sb = sin(phi * dtr);
F = [-sl -sb*cl cb*cl; F = [-sl -sb*cl cb*cl;
cl -sb*sl cb*sl; cl -sb*sl cb*sl;
0 cb sb]; 0 cb sb];
local_vector = F' * dx; local_vector = F' * dx;
E = local_vector(1); E = local_vector(1);
@ -54,4 +51,4 @@ if Az < 0
end end
D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2); D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
%%%%%%%%% end topocent.m %%%%%%%%% %%%%%%%%% end topocent.m %%%%%%%%%

23
src/utils/matlab/libs/geoFunctions/tropo.m
View File

@ -1,9 +1,9 @@
function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum) 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 % The range correction ddr in m is to be subtracted from
% pseudo-ranges and carrier phases % 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: % Inputs:
% sinel - sin of elevation angle of satellite % 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. % A Matlab reimplementation of a C code from driver.
% Kai Borre 06-28-95 % 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 a_e = 6378.137; % semi-major axis of earth ellipsoid
@ -60,14 +57,14 @@ while 1
% check to see if geometry is crazy % check to see if geometry is crazy
if rtop < 0 if rtop < 0
rtop = 0; rtop = 0;
end end
rtop = sqrt(rtop) - (a_e+hsta)*sinel; rtop = sqrt(rtop) - (a_e+hsta)*sinel;
a = -sinel/(htop-hsta); a = -sinel/(htop-hsta);
b = -b0*(1-sinel^2) / (htop-hsta); b = -b0*(1-sinel^2) / (htop-hsta);
rn = zeros(8,1); rn = zeros(8,1);
for i = 1:8 for i = 1:8
rn(i) = rtop^(i+1); rn(i) = rtop^(i+1);
end end
@ -77,17 +74,17 @@ while 1
b^2*(6*a^2+4*b)*1.428571e-1, 0, 0]; b^2*(6*a^2+4*b)*1.428571e-1, 0, 0];
if b^2 > 1.0e-35 if b^2 > 1.0e-35
alpha(7) = a*b^3/2; alpha(7) = a*b^3/2;
alpha(8) = b^4/9; alpha(8) = b^4/9;
end end
dr = rtop; dr = rtop;
dr = dr + alpha*rn; dr = dr + alpha*rn;
tropo = tropo + dr*ref*1000; tropo = tropo + dr*ref*1000;
if done == 'TRUE ' if done == 'TRUE '
ddr = tropo; ddr = tropo;
break; break;
end end
done = 'TRUE '; done = 'TRUE ';