clc;
clear all;
close all;
format long g;
[formatname,filename]=uigetfile('.txt','navigation');
g=load([filename,formatname]);
mu=3.986008*10^14;
We=7.292115147*10^-5;
s=size(g);
[filename,pathname]=uigetfile('*.*','please enter the observation file');
addpath(pathname);
temp=importdata(filename);
observation=temp.textdata;
time=zeros(length(observation)-17,6);
raw=1;
numbersatellite=0;
i=18;
for k=1:100
numbernamesatellites=observation{i,8};
[numbersatellite,namesatellite]=strtok(numbernamesatellites,'G');
numbersatellite=str2double(numbersatellite);
time(raw,1:6)=[str2double(observation{i,1}),str2double(observation{i,2}),str2double(observation{i,3}),str2double(observation{i,4}),str2double(observation{i,5}),...
str2double(observation{i,6})];
raw=raw+1;
i=i+numbersatellite+1;
if i>=length(observation)
break
end
end
time=time(1:raw-1,1:6);
JD2000=2451545.0;
GPSWEEK=zeros(length(g)/8,1);
for i=1:length(g)/8
GPSWEEK(i,1)=g(8*i-2,3);
end
for i=1:raw-1
a(i,1)=(14-time(i,2))/12;
y(i,1)=2000+time(i,1)+4800-a(i,1);
m(i,1)=time(i,2)+12*a(i,1)-3;
JDN(i,1)=time(i,3)+((153*m(i,1)+2)/5)+365*y(i,1)+((y(i,1)/4))-32083;
JD(i,1)=JDN(i,1)+((time(i,4)-12)/24)+((time(i,5))/1440)+(time(i,6)/86400);
tobs(i,1)=(JD(i,1)-JD2000-(7*GPSWEEK(1,1)))*86400;
end
delta_n=zeros(length(g)/8,1);
for i=1:length(g)/8
delta_n(i,1)=g(8*i-6,3);
end
M0=zeros(length(g)/8,1);
for i=1:length(g)/8
M0(i,1)=g(8*i-6,4);
end
e=zeros(length(g)/8,1);
for i=1:length(g)/8
e(i,1)=g(8*i-5,2);
end
a_square=zeros(length(g)/8,1);
for i=1:length(g)/8
a_square(i,1)=g(8*i-5,4);
end
t0e=zeros(length(g)/8,1);
for i=1:length(g)/8
t0e(i,1)=g(8*i-4,1);
end
OMEGA0=zeros(length(g)/8,1);
for i=1:length(g)/8
OMEGA0(i,1)=g(8*i-4,3);
end
i0=zeros(length(g)/8,1);
for i=1:length(g)/8
i0(i,1)=g(8*i-3,1);
end
w=zeros(length(g)/8,1);
for i=1:length(g)/8
w(i,1)=g(8*i-3,3);
end
OMEGAdot=zeros(length(g)/8,1);
for i=1:length(g)/8
OMEGAdot(i,1)=g(8*i-3,4);
end
Idot=zeros(length(g)/8,1);
for i=1:length(g)/8
Idot(i,1)=g(8*i-2,1);
end
Cuc=zeros(length(g)/8,1);
for i=1:length(g)/8
Cuc(i,1)=g(8*i-5,1);
end
Crc=zeros(length(g)/8,1);
for i=1:length(g)/8
Crc(i,1)=g(8*i-3,2);
end
Cus=zeros(length(g)/8,1);
for i=1:length(g)/8
Cus(i,1)=g(8*i-5,3);
end
Crs=zeros(length(g)/8,1);
for i=1:length(g)/8
Crs(i,1)=g(8*i-6,2);
end
Cic=zeros(length(g)/8,1);
for i=1:length(g)/8
Cic(i,1)=g(8*i-4,2);
end
Cis=zeros(length(g)/8,1);
for i=1:length(g)/8
Cis(i,1)=g(8*i-4,4);
end
tk=zeros(length(g)/8,1);
Mk=zeros(length(g)/8,1);
for j=1:raw-1
for i=1:length(g)/8
tk(i,1)=tobs(j,1)-t0e(i,1);
Mk(i,1)=M0(i,1)*((sqrt(mu/(a_square(i,1))^6))+delta_n(i,1))*tk(i,1);
end
Ek=zeros(length(g)/8,1);
Ek1=zeros(length(g)/8,1);
for k=1:1000
for i=1:length(g)/8
Ek1(i,1)=Mk(i,1)+e(i,1)*sin(Ek(i,1));
end
if max(Ek1(:,1)-Ek(:,1))<=10^-8
break
disp(k)
end
Ek(:,1)=Ek1(:,1);
end
fk=zeros(length(g)/8,1);
for i=1:length(g)/8
fk(i,1)=2*atan((sqrt(1-(e(i,1)^2))*sin(Ek(i,1)))/(cos(Ek(i,1))-e(i,1)));
end
uk=zeros(length(g)/8,1);
for i=1:length(g)/8
uk(i,1)=w(i,1)+fk(i,1)+(Cuc(i,1)*cos(2*(w(i,1)+fk(i,1)))+Cus(i,1)*sin(2*(w(i,1)+fk(i,1))));
end
rk=zeros(length(g)/8,1);
for i=1:length(g)/8
rk(i,1)=(a_square(i,1)^2)*(1-(e(i,1)*cos(Ek(i,1))))+Crc(i,1)*cos(2*(w(i,1)+fk(i,1)))+Crs(i,1)*sin(2*(w(i,1)+fk(i,1)));
end
ik=zeros(length(g)/8,1);
for i=1:length(g)/8
ik(i,1)=i0(i,1)+(Idot(i,1)*tk(i,1))+Cic(i,1)*cos(2*(w(i,1)+fk(i,1)))+Cis(i,1)*sin(2*(w(i,1)+fk(i,1)));
end
GAST=zeros(length(g)/8,1);
for i=1:length(g)/8
GAST(i,1)=(We*tk(i,1))+(We*t0e(i,1));
end
OMEGAk=zeros(length(g)/8,1);
for i=1:length(g)/8
OMEGAk(i,1)=OMEGA0(i,1)+((OMEGAdot(i,1)-We)*tk(i,1))-(We*t0e(i,1));
end
for i=1:length(g)/8
R3UK(3*i-2:3*i,1:3)=[cos(uk(i,1)) -sin(uk(i,1)) 0;sin(uk(i,1)) cos(uk(i,1)) 0;0 0 1];
R3OMEGAK(3*i-2:3*i,1:3)=[cos(OMEGAk(i,1)) -sin(OMEGAk(i,1)) 0;sin(OMEGAk(i,1)) cos(OMEGAk(i,1)) 0;0 0 1];
R3IK(3*i-2:3*i,1:3)=[cos(ik(i,1)) 0 sin(ik(i,1));0 1 0;-sin(ik(i,1)) 0 cos(ik(i,1))];
end
Rk=zeros(length(g)/8,1);
for i=1:length(g)/8
Rk(3*i-2:3*i,1)=[rk(i,1);0;0];
end
cartezian_coordinates=zeros(length(g)/8,1);
for i=1:length(g)/8
cartezian_coordinates(3*i-2:3*i,1)=R3OMEGAK(3*i-2:3*i,1:3)*R3UK(3*i-2:3*i,1:3)* R3IK(3*i-2:3*i,1:3)*Rk(3*i-2:3*i,1);
end
for i=1:length(g)/8
X(i,j)=cartezian_coordinates(3*i-2,1);
Y(i,j)=cartezian_coordinates(3*i-1,1);
Z(i,j)=cartezian_coordinates(3*i,1);
end
end
X=X';
Y=Y';
Z=Z';