mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 20:50:33 +00:00
Clean up Matlab/Octave code
This commit is contained in:
Carles Fernandez
parent
c58107d56c
commit
c9b2f06d41
@ -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;
|
||||
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
|
||||
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 %%%%%%%%%%%%%%%%%%%
|
||||
|
||||
@ -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
|
||||
@ -69,77 +66,77 @@ while abs(U-oldU) > 1.e-4
|
||||
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);
|
||||
|
||||
@ -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
|
||||
@ -25,4 +22,5 @@ if time > half_week
|
||||
elseif time < -half_week
|
||||
corrTime = time + 2*half_week;
|
||||
end
|
||||
|
||||
%%%%%%% end check_t.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);
|
||||
|
||||
@ -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 %%%%%%%%%%%%%%%%%%%%%
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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,8 +22,8 @@ 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;
|
||||
|
||||
@ -1,13 +1,13 @@
|
||||
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
|
||||
@ -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))
|
||||
|
||||
@ -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
|
||||
@ -23,11 +23,8 @@ function [X, Y, Z] = geo2cart(phi, lambda, h, i)
|
||||
% 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;
|
||||
@ -45,4 +42,5 @@ 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);
|
||||
|
||||
%%%%%%%%%%%%%% end geo2cart.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;] -
|
||||
@ -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 =======================================================
|
||||
@ -53,7 +50,7 @@ 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) --------
|
||||
@ -65,7 +62,7 @@ for iter = 1:nmbOfIterations
|
||||
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;
|
||||
@ -77,9 +74,9 @@ for iter = 1:nmbOfIterations
|
||||
|
||||
%--- 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
|
||||
|
||||
@ -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,7 +23,7 @@ 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
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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,12 +18,9 @@ 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);
|
||||
@ -31,12 +28,12 @@ numOfSatellites = size(prnList, 2);
|
||||
% GPS constatns
|
||||
|
||||
gpsPi = 3.1415926535898; % Pi used in the GPS coordinate
|
||||
% system
|
||||
% 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,19 +46,19 @@ 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;
|
||||
@ -123,7 +120,7 @@ for satNr = 1 : numOfSatellites
|
||||
|
||||
%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);
|
||||
|
||||
@ -133,9 +130,9 @@ for satNr = 1 : numOfSatellites
|
||||
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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -1,9 +1,9 @@
|
||||
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;])
|
||||
@ -14,11 +14,8 @@ function [Az, El, D] = topocent(X, dx)
|
||||
% 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;
|
||||
@ -31,8 +28,8 @@ 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);
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user