ANEXO D. Funciones en Matlab utilizadas.

Funcin ode45.m
La funcin ode45 de Matlab nos permite resolver ecuaciones diferenciales y
sistemas de ecuaciones diferenciales por medio de mtodos numricos
basndose en el mtodo de Dormand-Prince. Este mtodo numrico de siete
etapas, pertenece a la familia de mtodos Runge-Kutta, y permite calcular
soluciones de cuarto y quinto orden. Evala seis veces la funcin por paso,
debido a que la etapa siete de un paso se evala en el mismo punto que el
primero del paso siguiente. Es un mtodo muy adecuado usar en interpolacin
local (cuando la solucin de orden superior es utilizada para continuar la
integracin) debido a que minimiza el error de la solucin de quinto orden a
diferencia de mtodos como el de Fehlberg que lo hace para la solucin de
cuarto orden.
La sintaxis en Matlab es:
[ t , x ] =ode 45 (odefun ,tspan , x 0, options , params)
x:

Matriz con variables independientes en cada columna

t:

Vector tiempo
odefun: Nombre de la funcin
tspan : Se pasa a la funcin el intervalo deseado de tiempo. Puede
especificarse como dos valores de tiempo (inicial y final) mediante la sintaxis
tspan=[ti , tf ] o bien puede especificarse los instantes en los que se quieren
los valores de las variables dependientes como tspan=[ t 0, t 1, tn ] .
x 0 : Vector de valores iniciales
options :

Estructura opcional que permite entre otras opciones establecer la

tolerancia del error de la siguiente manera:
options=odeset (' RelTo l ' , 1012 , ' Absto l ' , 1012)
params:

Parmetros que queremos pasar a la funcin.

Funcin Vectorperifocal.m
La funcin Vectorperifocal nos va a computar el vector de estado (r,v) en el
marco geocntrico ecuatorial a partir de los elementos orbitales definidos por
el vector [coe].
Los elementos orbitales necesarios que hay que pasar a la funcin como datos
iniciales son:
h: momento angular (km^2/s)
e: excentricidad

RA: ascensin recta del nodo ascendente (grados decimales).

i: inclinacin de la rbita (grados decimales)
w: argumento del perigeo (grados decimales)
TA: anomala verdadera (grados decimales)
A travs de las matrices de rotacin entre marcos de referencia computar el
vector de estado (r,v) (km y km/s, respectivamente).
Habiendo creado un vector llamado coe con los elementos orbitales
especificados, la sintaxis necesaria para llamar a la funcin es:
[ r , v ] =Vectorperifocal [ coe ]

El cdigo de la funcin lo podemos ver a continuacin:

%------------------------------------------------------------------------function [r, v] = vectorperifocal(coe)
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%{
Esta funcion calcula el vector de estado en el marco geocentrico
(r,v) a partir del vector de los elementos orbitales clasicos(coe).
mu
coe

- parammetro gravitatorio terrestre (km^3;s^2)

- elementos orbitales [h e RA i w TA]

h
= momento angular (km^2/s)
e
= excentricidad
RA
= ascension recta del nodo ascendente (rad)
i
= inclinacion de la orbita (rad)
w
= argumento del perigeo (rad)
TA
= anomalia verdadera (rad)
R3_w - Matriz de rotacion sobre el eje-z a traves del angulo w
R1_i - Matriz de rotacion sobre el eje-x a traves del angulo i
R3_W - Matriz de rotacion sobre el eje-z a traves del angulo RA
Q_pX - Matriz de transformacion del marco de referencia perifocal al
marco de referencia ecuatorial
rp
- vector de posicion en el marco perifocal (km)
vp
- vector de velocidad en el marco perifocal (km/s)
r
- vector de posicion en el marco geocentrico ecuatorial (km)
v
- vector velocidad en el marco geocentrico ecuatorial (km/s)

%}
% ---------------------------------------------global mu;
mu=398600;
h
e
RA
i
w
TA

=
=
=
=
=
=

coe(1);
coe(2);
coe(3);
coe(4);
coe(5);
coe(6);

%...Calculo de vectores columna rp y vp:

rp = (h^2/mu) * (1/(1 + e*cos(TA))) * (cos(TA)*[1;0;0] +
sin(TA)*[0;1;0]);

vp = (mu/h) * (-sin(TA)*[1;0;0] + (e + cos(TA))*[0;1;0]);

%...Calculo de las matrices de rotacion:
R3_W = [ cos(RA) sin(RA) 0
-sin(RA) cos(RA) 0
0
0
1];
R1_i = [1
0
0

0
cos(i)
-sin(i)

R3_w = [ cos(w)
-sin(w)
0

0
sin(i)
cos(i)];

sin(w)
cos(w)
0

0
0
1];

%Matriz de rotacion del marco de referencia perifocal al marco de

%referencia ecuatorial
Q_pX = (R3_w*R1_i*R3_W)';
%...Vectores columna r y v en el marco de referencia ecuatorial:
r = Q_pX*rp;
v = Q_pX*vp;
%...COnversion de vectores r y v a vectores fila:
r = r';
v = v';
end
%-----------------------------------------------------------------------

Funcin vectorestadotiempo.m
La funcin vectorestadotiempo va a calcular el vector de estado [R,V] en el
marco de referencia geocntrico ecuatorial a partir de un vector de estado
inicial [r0,v0] en el marco de referencia ecuatorial pasado un intervalo de
tiempo t.
Los valores de entrada de la funcin son:
1 Un vector de posicin inicial R0 de una fila y tres columnas
2 Un vector de velocidad inicial V0 de una fila y tres columnas
3 Un valor del intervalo de paso de tiempo t.
Esta funcin requiere las funciones adicionales Kepler.m y
coeficientesdeLagrange.m que son explicadas a continuacin de esta.
Los pasos de esta funcin son los siguientes:
1 Calcula los valores necesarios de velocidad inicial radial y el inverso del
semieje mayor necesarios para computar la anomala universal.
2 Calculo de la anomala universal mediante la funcin Kepler.m
3 Calculo de los coeficientes de Lagrange, sus derivadas y el vector de
posicin final R mediante la funcin coeficientesdeLagrange.m

4 Calculo del vector velocidad final a partir de las derivadas de los

coeficientes de Lagrange previamente calculados.

El cdigo de la funcin es el siguiente:

function [R,V] = vectorestadotiempo(R0, V0, t)
%
%{
Esta funcin calcular el vector de estado [R,V] en el marco de
referencia ecuatorial a partir de un vector de estado inicial [r0,v0]
en el marco de referencia ecuatorial y un intervalo de tiempo
transcurrido.
mu - parametro gravitacional (km3/s2)
R0 - vector de posicion inicial (km)
V0 - vector de velocidad inicial (km/s)
t - intervalo de tiempo transcurrido (s)
R - Vector de posicion final (km)
V - Vector de velocidad final (km/s)
Este programa requiere las funciones
adicionales: kepler.m, coeficientesdeLagrange.m,
%}
% ---------------------------------------------global mu
mu=398600;
%...Magnitudes of R0 and V0:
r0 = norm(R0);
v0 = norm(V0);
%...Calculo de la velocidad inicial radial:
vr0 = dot(R0, V0)/r0;
%...Calculo del inverso del semieje mayor
alpha = 2/r0 - v0^2/mu;
%...Calculo de la anomalia universal
x = kepler(t, r0, vr0, alpha);
%...Calculo de los coeficientes de Lagrange f and g y
% sus derivadas y el vector de posicion final R:
[f, g, fdot, gdot, R] = coeficientesdeLagrange(x, t, r0, alpha, R0,
V0);
%...Calculo de la velocidad final V :
V = fdot*R0 + gdot*V0

Funcin Kepler.m
La funcin Kepler nos permite resolver la ecuacin de Kepler universal para
la anomala universal vista en el captulo 2 a travs del mtodo iterativo
explicado de Newton.
Los parmetros iniciales requeridos por la funcin son:
a: semieje mayor
vro: velocidad inicial radial
ro: norma del vector de posicin R0

dt: intervalo de tiempo desde el paso por el perigeo.

Obtendremos un valor8de salida correspondiente a la anomala universal con
una tolerancia de 10
(por defecto).
Requiere la funcin adicional de stumpff.m que permite la evaluacin de
las funciones de Stumpff C(z) y S(z) vistas en el captulo 2.
El cdigo de la funcin puede verse a continuacin:
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
function x = kepler(dt, ro, vro, a)
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%{
Esta funcion resuleve la ecuacion de Kepler universal para anomalia
universal a partir del metodo iterativo de Newton
mu
x
dt
ro
vro
a
z
C
S
n
nMax

parametro gravitacional (km^3/s^2)

anomalia universal (km^0.5)
tiempo desde x = 0 (s)
posicion radial (km) when x = 0
velocidad radial (km/s) when x = 0
inverso del semieje mayor (1/km)
variable auxiliar (z = a*x^2)
valor de la funcion de Stumpff
valor de la funcion de Stumpff
numero de iteraciones para la convergencia de la solucion
maximo numero de iteraciones permitidas

Funciones adicionales requeridas: stumpff.m

%}
% ---------------------------------------------global mu
%...Establecer la tolerancia y un numero maximo de iteraciones:
error = 1.e-8;
nMax = 1000;
%...Valor inicial para x (Chobotov):
x = sqrt(mu)*abs(a)*dt;
%...Iteracion hasta convergencia
%...Error de la tolerancia:
n
= 0;
ratio = 1;
while abs(ratio) > error && n <= nMax
n
= n + 1;
[C,S] = stumpff(a*x^2);
F
= ro*vro/sqrt(mu)*x^2*C + (1 - a*ro)*x^3*S + ro*x sqrt(mu)*dt;
dFdx = ro*vro/sqrt(mu)*x*(1 - a*x^2*S) + (1 - a*ro)*x^2*C + ro;
ratio = F/dFdx;
x
= x - ratio;
end
%...Se muestra un valor de x, pero se informa que el nMax fue
alcanzado:
if n > nMax
fprintf('\n **No. iterations of Kepler''s equation = %g', n)
fprintf('\n
F/dFdx
= %g\n', F/dFdx)

end
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Funcin Stumpff.m
Permite2 evaluar las funciones C(z) y S(z) de Stumpff para un valor de entrada
z= .
Los valores de entrada necesarios son el valor de z.
Devuelve dos valores:
1 C: Evaluacin de la funcin C(z).
2 S: Evaluacin de la funcin S(z).
El cdigo puede verse a continuacin:
function [C,S] = stumpff(z)
%
%
% Esta funcion evalua las funciones C(z) y S(z) de Stumpff
%
% z - argumento de entrada
% C - Valor de salida de C(z)
% S - Valor de salida de S(z)
% -----------------------------------------------------------if z > 0
C = (1 - cos(sqrt(z)))/z;
S = (sqrt(z) - sin(sqrt(z)))/(sqrt(z))^3;
elseif z < 0
C = (cosh(sqrt(-z)) - 1)/(-z);
S = (sinh(sqrt(-z)) - sqrt(-z))/(sqrt(-z))^3;
else
C = 1/2;
S = 1/6;
end

Funcin coeficientesdeLagrange.m
Esta funcin permite computar los coeficientes de Lagrange, sus derivadas y
el vector de posicin R final para usar con la funcin
Vectorestadotiempo.m.
Los valores necesarios a introducir en la funcin como parmetros de entrada
son:
1 La anomala universal tras un tiempo t (procedente de la solucin de la
ecuacin de Kepler descrita anteriormente).
2 El inverso del semieje mayor
3 La posicin radial para el instante to inicial

4 El vector de posicin inicial R0

5 El vector de velocidad final V0
Devuelve un vector formado por los coeficientes de Lagrange, sus derivadas y
el vector de posicin final R.
Necesita la funcin stumpff.m ya explicada anteriormente.
Puede verse el cdigo de la funcin a continuacin:
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
function [f, g, fdot, gdot, R] = coeficientesdeLagrange(x, t, ro,
a,R0,V0)
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%{
Esta funcion calcula los coeficientes de Lagrange, sus derivadas.
mu - parametro gravitacional (km^3/s^2)
a - inverso del semieje mayor (1/km)
ro - posicion radial para el instante to (km)
t - tiempo pasado desde ro (s)
x - anomalia universal despues del tiempo t (km^0.5)
f - coeficiente de Lagrange f (dimensionless)
g - coeficiente de Lagrange g (s)
f_dot - derivada del coeficiente de Lagrange f
g_dot - derivada del coeficiente de Lagrange g

Funciones adicionales requeridas: stumpff.m

%}
% ---------------------------------------------global mu
z = a*x^2;
[C,S]=stumpff(z);
%Coeficientes de Lagrange
f = 1 - x^2/ro*C;
g = t - 1/sqrt(mu)*x^3*S;
%Calculo adicional del vector de posicion final R (para usar con
%funcion vectorestado.m).
R = f*R0 + g*V0;
r = norm(R);
%Derivados de los coeficientes
fdot = sqrt(mu)/r/ro*(z*S - 1)*x;
gdot = 1 - x^2/r*C;
end
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Funcin eul2quat.m
La funcin eul2quat convierte los ngulos de Euler en un cuaternin.
La funcin pide como parmetros de entrada 3 valores respectivos para los
tres ngulos de Euler y devuelve un vector con el cuaternin o bien tres

vectores de n filas y 1 columna para convertir n triadas de ngulos de Euler a

sus respectivos cuaterniones.
Es necesario introducir los ngulos en grados en radianes
La sintaxis correspondiente sera la siguiente:
[ q ]=eul2 quat (e 1,e2,e3)
Donde
e1:

Primer ngulo de Euler o vector de n filas y 1 columna

correspondientes al primer ngulo de Euler ( .
e2:
Segundo ngulo de Euler o vector de n filas y 1 columna
correspondientes al segundo ngulo de Euler () .
e3:

Tercer ngulo de Euler o vector de n filas y 1 columna correspondientes

al tercer ngulo de Euler () .
q : Vector con el cuaternio o con n cuaterniones correspondientes a los tres
ngulos de Euler o n triadas de ngulos de Euler introducidos.
El cdigo de la funcin se puede ver a continuacin:
function [q] = eul2quat(e1, e2, e3)
%{
Funcion que convierte los angulos de Euler
cuaternion.
Las entradas necesarias son:
e1: n x 1 Vector de angulos de Euler 1
e2: n x 1 Vector de angulos de Euler 2
e3: n x 1 Vector de angulos de Euler 3

desde grados decimales al

(phi1)
(PHI)
(phi2)

%Salida:
%
q: Cuaternion correspondiente a la rotacion descrita por los
angulos de Euler.
%}
%Redefiniendo fi, teta y chi en radianes
%Calculo del cuaternion
for i = 1:length(e1)
q(i,:) = [cos(e2(i)/2)*cos((e1(i) + e3(i))/2);
-sin(e2(i)/2)*sin((e1(i) - e3(i))/2); sin(e2(i)/2)*cos((e1(i) e3(i))/2); cos(e2(i)/2)*sin((e1(i) + e3(i))/2)]';
end

Funcin quat2eul.m
La funcin quat2eul permite convertir los cuaterniones a ngulos de Euler por
medio de las relaciones trigonomtricas existentes entre ellos y vistas a lo
largo del proyecto. El cdigo es el siguiente:
%------------------------------------------------------------------------function [euler] = quat2eul(q)

%
%Funcion que convierte los cuaterniones a angulos de euler.
%
%Entrda
% Una matriz 4 x n de cuaterniones
%
%%Salida
%
euler: una matriz n x 3 de angulos de Euler correspondientes al
cuaternion introducido
% (fi teta chi)
%
%Relaciones trigonometricas usadas:
%q0 = cos((fi+chi)/2)cos(teta/2);
%q1 = cos((fi-chi)/2)sin(teta/2);
%q2 = sin((fi-chi)/2)sin(teta/2);
%q3 = sin((fi+chi)/2)cos(teta/2);
%
%Separacion del cuaternion en componentes
q0 = q(1,:);
q1 = q(2,:);
q2 = q(3,:);
q3 = q(4,:);
%Calculo de fi + chi y fi - chi
p1pp2 = atan2(q3,q0);
p1mp2 = atan2(q2,q1);
%Calculo de los angulos de Euler
chi = p1pp2 - p1mp2;
teta = 2*atan2(sqrt(q1.^2+q2.^2),sqrt(q0.^2+q3.^2));
fi = p1pp2 + p1mp2;
%Si el segundo angulo de Euler es cero entonces todas las rotaciones
pueden
%ser dadas por el primero o ultimo angulo de Euler
tetacero = abs(teta)<=1E-13;
%Tratar q1<0 cambiando el signo al cuaternion
changeref = ones(size(q0(tetacero)));
changeref(q0(tetacero)<0) = -1;
%Rotacion en torno a chi
chi(tetacero) = 2*asin(max(-1,min(1,changeref).*q3(tetacero)));
fi(tetacero) = 0;
%Salida en radianes
euler = [fi' teta' chi'];
end
%-------------------------------------------------------------------------

Funcin magfield.m
La funcin magfiel.m nos va a permitir calcular el vector campo magntico
terrestre en una ubicacin y una poca a partir del modelo WMM descrito en
el captulo 4. Los valores de entrada al programa son un vector de posicin, la
fecha exacta [ao, mes, dia, hora, minuto, segundo] el grado de expansin

del modelo (hasta 12 como vimos), un intervalo en segundos (1s) y el tipo,

esto es, las coordenadas en las que se ha introducido el vector de posicin r (1
para sistema de referencia geocntrico inercial, como es nuestro caso).
Podremos obtener varias salidas en el programa, el campo magntico en
distintos sistemas de referencia, la intensidad total del campo, la declinacin,
la inclinacin, etc, pero la ms importante y la nica que utilizaremos en
nuestro caso ser la primera, el campo magntico en coordenadas en el
sistema geocntrico inercial. Obtendremos el vector en nanoTeslas.
Dentro de la funcin existe a su vez una subfuncin llamda wmm2015_data.
Esta funcin contiene todos los coeficientes armnicos esfricos del modelo
WMM2015 actualizados y vlidos hasta el 2020, que se pueden obtener en la
referencia [31].
Si quisisemos obtener el campo magntico para otra poca debemos
encontrar y sustituir los correspondientes coeficientes en la funcin.
Para la validacin del cdigo se sustituyeron los coeficientes por los del
modelo WMM2010.
El cdigo de la funcin puede verse a continuacin:
function
[b_eci,b_ecef,b_ned,ti,bh,dec,dip]=mag_field(r,yr,mth,day,hr,min,sec,m
ln,dt,type);
%
[b_eci,b_ecef,b_ned,ti,bh,dec,dip]=mag_field(r,yr,mth,day,hr,min,sec,m
ln,dt,type);
%
% This program computes the reference magnetic field using the WMM
model.
%
% The inputs are:
%
r = position vector (see type below)
%
yr = year (e.g. 2015)
%
mth = month
%
day = day
%
hr = hour (0-23)
%
min = minute (0-59)
%
sec = second (0-59)
%
mln = field order (up to 12)
%
dt = sampling interval in seconds (will be used for m > 1)
%
type = 1 for r in ECI coordinates in km
%
= 2 for r in ECEF coordinates in km
%
= 3 for r in [lat long height] in [deg deg meter]
%
% The outputs are:
%
b_eci = magnetic field in ECI coordinates in nT (m x 3)
% b_ecef = magnetic field in ECEF coordinates in nT (m x 3)
%
b_ned = magnetic field in NED coordinates in nT (m x 3)
%
ti = total field intensity in nT (m x 1)
%
bh = horizontal field intensity in nT (m x 1)
%
dec = declination in deg (m x 1)
%
dip = inclination in deg (m x 1)
% John L. Crassidis 5/28/06

% Note: this program was converted from FORTRAN code provided by the
% National Geophysical Data Center. See
http://www.ngdc.noaa.gov/seg/WMM/.
% Epoch
epoch=2015.00;
% Load Data for 2015
wmm=wmm2015_data;
% Constants for Degrees/Radians Conversions
dtr=pi/180;
rtd=180/pi;
% Get Length of Data
[mout,mout3]=size(r);
if mout3~=3, disp('r must be (m x 3)');end
% Initialize Field Vectors
bt_store=zeros(mout,1);br_store=zeros(mout,1);bp_store=zeros(mout,1);
t=[0:dt:(mout-1)*dt]';
% Convert from ECI to Lat, Long and Height if Needed
if type == 1
r_ecef=eci2ecef(r,yr,mth,day,hr,min,sec+t);
[lat,long,height]=ecef2llh(r_ecef);height=height/1000;
end
% Convert from ECEF to Lat, Long and Height if Needed
if type == 2
[lat,long,height]=ecef2llh(r);height=height/1000;
end
% Get Lat, Long and Height
if type == 3
lat=r(:,1);
long=r(:,2);
height=r(:,3)/1000;
end
% Convert to Radians
rlon=long*dtr;
rlat=lat*dtr;
% Get Sidereal Time and Ascending Node
theta=sidereal(yr,mth,day,hr,min,sec+t)*dtr;
alpha=rlon+theta;
c_alpha=cos(alpha);s_alpha=sin(alpha);
% Leap Year
if (mod(yr,400)&&~mod(yr,100))
% leapyear = false;
ndays = 365;
elseif ~mod(yr,4)
% leapyear = true;
ndays = 366;
else
% leapyear = false;
ndays = 365;

end
% Get Days Past First of Year
o1=ones(mout,1);
day_of_year = datenum(yr*o1,mth*o1,day*o1,hr*o1,min*o1,sec+t)datenum(yr,1,1);
% Find Days That Go Into Next Year
i_days=find(day_of_year > ndays);
% Get Decimal Year Time
if isempty(i_days) == 1
time = yr + day_of_year/ndays;
else
% Check Leap Year of New Year
time = yr + day_of_year/ndays;
yr_new=floor(time(i_days(1)));
if (mod(yr_new,400)&&~mod(yr_new,100))
% leapyear = false;
ndays_new = 365;
elseif ~mod(yr_new,4)
% leapyear = true;
ndays_new = 366;
else
% leapyear = false;
ndays_new = 365;
end
time=zeros(mout,1);
time(1:i_days(1)-1) = yr + day_of_year(1:i_days(1)-1)/ndays;
time(i_days(1):mout) = yr + (day_of_year(i_days:mout)ndays+ndays_new)/ndays_new;
end
% Get Spherical Harmonic Coefficients from Data
c=zeros(13,13);cd=zeros(13,13);
for i=1:length(wmm)
n=wmm(i,1);
m=wmm(i,2);
if (m <= n)
c(n+1,m+1)=wmm(i,3);
cd(n+1,m+1)=wmm(i,5);
end
if (m ~= 0)
c(m,n+1)=wmm(i,4);
cd(m,n+1)=wmm(i,6);
end
end
% Convert Schmidt Normalized Gauss Coefficients to Unnormalized
snorm=zeros(mln+1,mln+1);
k=zeros(mln+1,mln+1);
fn=zeros(mln+1,1);
fm=zeros(mln+1,1);
snorm(1,1)=1;
for n=2:mln+1
snorm(n,1)=snorm(n-1,1)*(2*(n-1)-1)/(n-1);
j=2;
for m=1:n
k(n,m)=((n-2)^2-(m-1)^2)/((2*(n-1)-1)*(2*(n-1)-3));

if m > 1
flnmj=((n-m+1)*j)/(n+m-2);
snorm(n,m)=snorm(n,m-1)*sqrt(flnmj);
j=1;
c(m-1,n)=snorm(n,m)*c(m-1,n);
cd(m-1,n)=snorm(n,m)*cd(m-1,n);
end
c(n,m)=snorm(n,m)*c(n,m);
cd(n,m)=snorm(n,m)*cd(n,m);
end
fn(n)=n;
fm(n)=n-1;
end
k(2,2)=0;
% Constant for WGS-84
a=6378.137;
b=6356.7523142;
re=6371.2;
a2=a^2;
b2=b^2;
c2=a2-b2;
a4=a2^2;
b4=b2^2;
c4=a4-b4;
% Sine and Cosine of Lat and Long
srlon=sin(rlon);
srlat=sin(rlat);
crlon=cos(rlon);
crlat=cos(rlat);
srlon2=srlon.^2;
srlat2=srlat.^2;
crlon2=crlon.^2;
crlat2=crlat.^2;
% Convert from Geodetic to Spherical Coordinates
q=(a2-c2*srlat2).^(0.5);
q1=height.*q;
q2=((q1+a2)./(q1+b2)).^2;
ct=srlat./((q2.*crlat2+srlat2).^(0.5));
st=(1-ct.^2).^(0.5);
r2=height.^2+2*q1+(a4-c4*srlat2)./(q.^2);
r=(r2).^(0.5);
d=(a2*crlat2+b2*srlat2).^(0.5);
ca=(height+d)./r;
sa=c2*crlat.*srlat./(r.*d);
% Main Loop
for i = 1:mout
% Update Time
dt_change=time(i)-epoch;
% Initialize Quantities
sp=zeros(13,1);
cp=zeros(13,1);
p=zeros(13,13);
dp=zeros(13,13);
pp=zeros(13,1);

p(1,1)=1;
pp(1)=1;
dp(1,1)=0;
sp(1)=0;
cp(1)=1;
sp(2)=srlon(i);
cp(2)=crlon(i);
for m=3:mln+1;
sp(m)=sp(2)*cp(m-1)+cp(2)*sp(m-1);
cp(m)=cp(2)*cp(m-1)-sp(2)*sp(m-1);
end
aor=re/r(i);
ar=aor^2;
br=0;
bt=0;
bp=0;
bpp=0;
tc=zeros(mln+1,mln+1);
% Loop for n and m
for n=2:mln+1
ar=ar*aor;
for m=1:n
% Compute Unnormalized Associated Legendre Polynomials and Derivatives
if (n == m)
p(n,m)=st(i)*p(n-1,m-1);
dp(n,m)=st(i)*dp(n-1,m-1)+ct(i)*p(n-1,m-1);
elseif (n == 2) & (m == 1)
p(n,m)=ct(i)*p(n-1,m);
dp(n,m)=ct(i)*dp(n-1,m)-st(i)*p(n-1,m);
elseif (n > 2) & (n ~= m)
if (m > n-2)
p(n-2,m)=0;
dp(n-2,m)=0;
end
p(n,m)=ct(i)*p(n-1,m)-k(n,m)*p(n-2,m);
dp(n,m)=ct(i)*dp(n-1,m)-st(i)*p(n-1,m)-k(n,m)*dp(n-2,m);
end
% Time Adjust the Gauss Coefficients
tc(n,m)=c(n,m)+dt_change*cd(n,m);
if (m ~= 1)
tc(m-1,n)=c(m-1,n)+dt_change*cd(m-1,n);
end
% Accumulate Terms of the Spherical Harmonic Expansions
par=ar*p(n,m);
if (m == 1)
temp1=tc(n,m)*cp(m);
temp2=tc(n,m)*sp(m);
else
temp1=tc(n,m)*cp(m)+tc(m-1,n)*sp(m);

temp2=tc(n,m)*sp(m)-tc(m-1,n)*cp(m);
end
bt=bt-ar*temp1*dp(n,m);
bp=bp+fm(m)*temp2*par;
br=br+fn(n)*temp1*par;
% Special Case: North/South Geographic Poles
if (st(i) == 0) & (m == 2)
if (n == 2)
pp(n)=pp(n-1);
else
pp(n)=ct(i)*pp(n-1)-k(n,m)*pp(n-2);
end
parp=ar*pp(n);
bpp=bpp+fm(m)*temp2*parp;
end
end
end
if (st(i) == 0)
bp=bpp;
else
bp=bp/st(i);
end
% Store Field Variables
bt_store(i)=bt;
br_store(i)=br;
bp_store(i)=bp;
end
% Rotate from Spherical to Geodetic Coordinates
b_ned=[-bt_store.*ca-br_store.*sa bp_store bt_store.*sa-br_store.*ca];
% Get ECI Coordinates
brbt=(br_store.*st+bt_store.*ct);
b_eci=[brbt.*c_alpha-bp_store.*s_alpha brbt.*s_alpha+bp_store.*c_alpha
br_store.*ct-bt_store.*st];
% Horizontal, Total Field, Declination and Dip
bh=(b_ned(:,1).^2+b_ned(:,2).^2).^(0.5);
ti=(bh.^2+b_ned(:,3).^2).^(0.5);
dec=atan2(b_ned(:,2),b_ned(:,1))*rtd;
dip=atan2(b_ned(:,3),bh)*rtd;
% Get ECEF Coordinates
b_ecef=eci2ecef(b_eci,yr,mth,day,hr,min,sec+t);
return
function out=wmm2015_data
out=[1
1 1
2 0
2 1
2 2
3 0

-29438.5
0.0
-1501.1
4796.2
-2445.3
0.0
3012.5
-2845.6
1676.6
-642.0
1351.1
0.0

10.7
17.9
-8.6
-3.3
2.4
3.1

0.0

-26.8
0.0
-27.1
-13.3
0.0

3 1
3 2
3 3
4 0
4 1
4 2
4 3
4 4
5 0
5 1
5 2
5 3
5 4
5 5
6 0
6 1
6 2
6 3
6 4
6 5
6 6
7 0
7 1
7 2
7 3
7 4
7 5
7 6
7 7
8 0
8 1
8 2
8 3
8 4
8 5
8 6
8 7
8 8
9 0
9 1
9 2
9 3
9 4
9 5
9 6
9 7
9 8
9 9
10 0
10 1
10 2
10 3
10 4
10 5
10 6
10 7
10 8
10 9
10 10
11 0
11 1

-2352.3
1225.6
581.9
907.2
813.7
120.3
-335.0
70.3
-232.6
360.1
192.4
-141.0
-157.4
4.3
69.5
67.4
72.8
-129.8
-29.0
13.2
-70.9
81.6
-76.1
-6.8
51.9
15.0
9.3
-2.8
6.7
24.0
8.6
-16.9
-3.2
-20.6
13.3
11.7
-16.0
-2.0
5.4
8.8
3.1
-3.1
0.6
-13.3
-0.1
8.7
-9.1
-10.5
-1.9
-6.5
0.2
0.6
-0.6
1.7
-0.7
2.1
2.3
-1.8
-3.6
3.1
-1.5

-115.3
245.0
-538.3
0.0
283.4
-188.6
180.9
-329.5
0.0
47.4
196.9
-119.4
16.1
100.1
0.0
-20.7
33.2
58.8
-66.5
7.3
62.5
0.0
-54.1
-19.4
5.6
24.4
3.3
-27.5
-2.3
0.0
10.2
-18.1
13.2
-14.6
16.2
5.7
-9.1
2.2
0.0
-21.6
10.8
11.7
-6.8
-6.9
7.8
1.0
-3.9
8.5
0.0
3.3
-0.3
4.6
4.4
-7.9
-0.6
-4.1
-2.8
-1.1
-8.7
0.0
-0.1

-6.2
-0.4
-10.4
-0.4
0.8
-9.2
4.0
-4.2
-0.2
0.1
-1.4
0.0
1.3
3.8
-0.5
-0.2
-0.6
2.4
-1.1
0.3
1.5
0.2
-0.2
-0.4
1.3
0.2
-0.4
-0.9
0.3
0.0
0.1
-0.5
0.5
-0.2
0.4
0.2
-0.4
0.3
0.0
-0.1
-0.1
0.4
-0.5
-0.2
0.1
0.0
-0.2
-0.1
0.0
0.0
-0.1
0.3
-0.1
-0.1
-0.1
0.0
-0.2
-0.1
-0.2
0.0
0.0

8.4
-0.4
2.3
0.0
-0.6
5.3
3.0
-5.3
0.0
0.4
1.6
-1.1
3.3
0.1
0.0
0.0
-2.2
-0.7
0.1
1.0
1.3
0.0
0.7
0.5
-0.2
-0.1
-0.7
0.1
0.1
0.0
-0.3
0.3
0.3
0.6
-0.1
-0.2
0.3
0.0
0.0
-0.2
-0.1
-0.2
0.1
0.1
0.0
-0.2
0.4
0.3
0.0
0.1
-0.1
0.0
0.0
-0.2
0.1
-0.1
-0.2
0.1
-0.1
0.0
0.0

11
11
11
11
11
11
11
11
11
11
12
12
12
12
12
12
12
12
12
12
12
12
12

2
3
4
5
6
7
8
9
10
11
0
1
2
3
4
5
6
7
8
9
10
11
12

-2.3
2.1
-0.9
0.6
-0.7
0.2
1.7
-0.2
0.4
3.5
-2.0
-0.3
0.4
1.3
-0.9
0.9
0.1
0.5
-0.4
-0.4
0.2
-0.9
0.0

2.1
-0.7
-1.1
0.7
-0.2
-2.1
-1.5
-2.5
-2.0
-2.3
0.0
-1.0
0.5
1.8
-2.2
0.3
0.7
-0.1
0.3
0.2
-0.9
-0.2
0.7

-0.1
0.1
0.0
0.0
0.0
0.0
0.0
0.0
-0.1
-0.1
0.1
0.0
0.0
0.1
-0.1
0.0
0.1
0.0
0.0
0.0
0.0
0.0
0.0

0.1
0.0
0.1
0.0
0.0
0.1
0.0
-0.1
0.0
-0.1
0.0
0.0
0.0
-0.1
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0];

function [lat,long,height]=ecef2llh(r_ecef)
% [lat,long,height] = ecef2llh(r_ecef)
%
% This program converts ECEF quantities into long, lat and height.
%
% The input is:
%
r_ecef = ECEF quantities in km (m x 3)
%
% The outputs are:
%
lat = geodetic latitude in degrees (m x 1)
%
long = longitude in degrees (m x 1)
%
height = height in meters (m x 1)
% John L. Crassidis 6/7/06
% This program uses a closed-form solution, which is valid as long as
% does not fall within 43 km of the Earth center. Details can be found
at:
% Zhu, J., "Exact Conversion of Earth-Centered-Earth-Fixed Coordinates
to
% Geodetic Coordinates," Journal of Guidance, Control and Dynamics,
% Vol. 16, No. 2, March-April 1993, pp. 389-391.
% Check Size of Input Vector
rad2deg=180/pi;
[mout,mout1]=size(r_ecef);
if mout1 ~= 3
disp(' r_ecef must be m x 3, where m is the total number of points')
end
% Constant for WGS-84
a=6378.137;
b=6356.7523142;
e2=0.00669437999;
% Conversion Parameters

w=(r_ecef(:,1).^2+r_ecef(:,2).^2).^(0.5);
l=e2/2;m=(w/a).^2;n=((1-e2)*r_ecef(:,3)/b).^2;
i=-(2*l^2+m+n)/2;k=l^2*(l^2-m-n);
mnl2=m.*n*l^2;
q=(m+n-4*l^2).^3/216+mnl2;
d=((2*q-mnl2).*mnl2).^(0.5);
beta=i/3-(q+d).^(1/3)-(q-d).^(1/3);
t=((beta.^2-k).^(0.5)-(beta+i)/2).^(0.5)-sign(m-n).*((betai)/2).^(0.5);
w1=w./(t+l);z1=(1-e2)*r_ecef(:,3)./(t-l);
% Compute Longitude
jj0=find((w-r_ecef(:,1))==0);
jj=find((w-r_ecef(:,1))~=0);
long=zeros(mout,1);
if isempty(jj) == 0
long(jj,:)=2*atan((w(jj)-r_ecef(jj,1))./r_ecef(jj,2))*rad2deg;
end
if isempty(jj0) == 0
long(jj0,:)=zeros(length(jj0),1);
end
% Compute Latitude
j0=find(w==0);
j=find(w~=0);
lat=zeros(mout,1);height=zeros(mout,1);
if isempty(j) == 0
lat(j,:)=atan(z1(j)./((1-e2)*w1(j)))*rad2deg;
height(j,:)=sign(t(j)-1+l).*((w(j)-w1(j)).^2+(r_ecef(j,3)z1(j)).^2).^(0.5)*1000;
end
if isempty(j0) == 0
lat(j0)=sign(r_ecef(j0,3))*90;
h(j0)=sign(r_ecef(j0,3))*r_ecef(j0,3)-b;
end
return
function r_ecef=llh2ecef(lat,long,height)
% r_ecef = llh2ecef(lat,long,height)
%
% This program converts long, lat and height into ECEF quantities.
%
% The inputs are:
%
lat = geodetic latitude in degrees (m x 1)
%
long = longitude in degrees (m x 1)
%
height = height in meters (m x 1)
%
% The output is:
%
r_ecef = ECEF quantities in km (m x 3)
% John L. Crassidis 6/7/06
deg2rad=pi/180;
% Get Length of Data
mout=length(lat);
if length(long) ~= mout
disp(' Lat, Long and Height Must Have Same Length')
end

if length(height) ~= mout
disp(' Lat, Long and Height Must Have Same Length')
end
% Constant for WGS-84
a=6378.137;
b=6356.7523142;
e2=0.00669437999;
% Convert Lat and Long to Degrees and Height to Km
lat=lat(:)*deg2rad;
long=long(:)*deg2rad;
height=height(:)/1000;
% Compute ECEF Quantities
n=a./((1-e2*sin(lat).^2).^(0.5));
x=(n+height).*cos(lat).*cos(long);
y=(n+height).*cos(lat).*sin(long);
z=(n*(1-e2)+height).*sin(lat);
r_ecef=[x y z];
return
function r_eci=ecef2eci(r_ecef,yr,mth,day,hr,min,sec)
% r_eci=ecef2eci(r_ecef,yr,mth,day,hr,min,sec)
%
% This program converts ECEF into ECI quantities.
%
% The inputs are:
%
r_ecef = ECEF quantities (m x 3)
%
yr = year (e.g. 2005), can be (m x 1)
%
mth = month, can be (m x 1)
%
day = day, can be (m x 1)
%
hr = hour (0-23), can be (m x 1)
%
min = minute (0-59),can be (m x 1)
%
sec = second (0-59), can be (m x 1)
%
% The output is:
%
r_eci= ECEF quantities (m x 3)
% John L. Crassidis 6/7/06
% Sidereal Time is Computed using Eq. (12.4) from Meeus, J.,
% Astronomical Algorithms, 2nd Edition, Willmann-Bell, Inc.,
% Richmond, VA, 1998.
% Check Size of Input Vector
[mout,mout1]=size(r_ecef);
if mout1 ~= 3
disp(' r_ecef must be m x 3, where m is the total number of points')
end
% Compute Day-of-Year
doy = day + 31*(mth-1) - fix(2.2+0.4*mth).*(mth>2) +
(mth>2).*(~rem(yr,4));
% Compute Julian Date
jd = 2415020 + (yr-1900)*365 + fix((yr-1901)/4) + (doy-1) + 0.5 + ...
(3600*hr + 60*min + sec) / 86400;

% Compute T
t=(jd-2451545)/36525;
% Compute Sidereal Angle
theta=(280.46061837+360.98564736629*(jd-2451545)+0.000387933*t.^2t.^3/38710000)*pi/180;
% Compute ECI Components
cost=cos(theta);sint=sin(theta);
r_eci(:,1)=cost.*r_ecef(:,1)-sint.*r_ecef(:,2);
r_eci(:,2)=sint.*r_ecef(:,1)+cost.*r_ecef(:,2);
r_eci(:,3)=r_ecef(:,3);
return
function r_ecef=eci2ecef(r_eci,yr,mth,day,hr,min,sec)
% r_ecef=eci2ecef(r_eci,yr,mth,day,hr,min,sec)
%
% This program converts ECI into ECEF quantities.
%
% The inputs are:
%
r_eci= ECEF quantities (m x 3)
%
yr = year (e.g. 2005), can be (m x 1)
%
mth = month, can be (m x 1)
%
day = day, can be (m x 1)
%
hr = hour (0-23), can be (m x 1)
%
min = minute (0-59),can be (m x 1)
%
sec = second (0-59), can be (m x 1)
%
% The output is:
%
r_ecef = ECEF quantities (m x 3)
% John L. Crassidis 6/7/06
% Sidereal Time is Computed using Eq. (12.4) from Meeus, J.,
% Astronomical Algorithms, 2nd Edition, Willmann-Bell, Inc.,
% Richmond, VA, 1998.
% Check Size of Input Vector
[mout,mout1]=size(r_eci);
if mout1 ~= 3
disp(' r_eci must be m x 3, where m is the total number of points')
end
% Compute Day-of-Year
doy = day + 31*(mth-1) - fix(2.2+0.4*mth).*(mth>2) +
(mth>2).*(~rem(yr,4));
% Compute Julian Date
jd = 2415020 + (yr-1900)*365 + fix((yr-1901)/4) + (doy-1) + 0.5 + ...
(3600*hr + 60*min + sec) / 86400;
% Compute T
t=(jd-2451545)/36525;
% Compute Sidereal Angle

theta=(280.46061837+360.98564736629*(jd-2451545)+0.000387933*t.^2t.^3/38710000)*pi/180;
% Compute ECEF Components
cost=cos(theta);sint=sin(theta);
r_ecef(:,1)=cost.*r_eci(:,1)+sint.*r_eci(:,2);
r_ecef(:,2)=-sint.*r_eci(:,1)+cost.*r_eci(:,2);
r_ecef(:,3)=r_eci(:,3);
return
function theta=sidereal(yr,mth,day,hr,min,sec)
% theta = sidereal(yr,mth,day,hr,min,sec)
%
% This program computes the mean sidereal time in degrees.
%
% The inputs are:
%
yr = year (e.g. 2005), can be (m x 1)
%
mth = month, can be (m x 1)
%
day = day, can be (m x 1)
%
hr = hour (0-23), can be (m x 1)
%
min = minute (0-59),can be (m x 1)
%
sec = second (0-59), can be (m x 1)
%
% The output is:
%
theta = sidereal time in degrees (m x 3)
% John L. Crassidis 6/7/06
% Sidereal Time is Computed using Eq. (12.4) from Meeus, J.,
% Astronomical Algorithms, 2nd Edition, Willmann-Bell, Inc.,
% Richmond, VA, 1998.

% Compute Day-of-Year
doy = day + 31*(mth-1) - fix(2.2+0.4*mth).*(mth>2) +
(mth>2).*(~rem(yr,4));
% Compute Julian Date
jd = 2415020 + (yr-1900)*365 + fix((yr-1901)/4) + (doy-1) + 0.5 + ...
(3600*hr + 60*min + sec) / 86400;
% Compute T
t=(jd-2451545)/36525;
% Compute Sidereal Angle
theta=280.46061837+360.98564736629*(jd-2451545)+0.000387933*t.^2t.^3/38710000;
mout=length(theta);
% Make Sidereal Angle from -360 to 360 Degrees
if (theta(1)<-360)
theta=theta+abs(fix(theta(1)/360))*360;
end
if (theta(1)>360)
theta=theta-abs(fix(theta(1)/360))*360;
end

return