%%%%%%%%%%%%%%%%%% gps single_location solution %%%%%%%%%%%
%--------------------by yu jiejie---------------------%

clear;
pi=3.1415926535;
a_e=6378137;          %地球橢球的長半徑
e_e=1/298.257223563;  %地球橢球的第一偏心率
mu=3.986008e14;       %開普勒常數，單位為m3/s2
w_ie=7.292115147e-5;  %地球自轉平均角速率，單位rad/s

t_k=26579;

%計算橢球的卯酉圈曲率半徑N
B0 = 32.0389;
W=sqrt(1-e_e*2*sin(B0)^2);
N=a_e/W;

[Obs_types1,ant_delta1,approx_ecef1,ifound_types1,eof1] = anheader('1Nct_1540.07O');
xp0 = approx_ecef1(1);
yp0 = approx_ecef1(2);
zp0 = approx_ecef1(3);

eph = rinexe('Nct_1540.07N');
[noeph1,satnum1,svprn1,ca_pseudoranges1,L1_carrier1,L2_carrier1,p2_pseudoranges1,p1_pseudoranges1]=rinexn('1Nct_1540.07O');

%衛星號為5,12,14,18,22,30,31,時間07年6月3日，GPS時7時30分0秒
for t=1:noeph1
        t_k = t_k+1;
   for q=1:satnum1
       pse(q,1) = ca_pseudoranges1(t,svprn1(q));
       M0(q,1) = eph(3,svprn1(q));
       delta_n(q,1) = eph(5,svprn1(q));
       s_e(q,1) = eph(6,svprn1(q));
       sqrt_a(q,1) = eph(4,svprn1(q));
       omega0(q,1) = eph(16,svprn1(q));
       i0(q,1) = eph(12,svprn1(q));
       s_w(q,1) = eph(7,svprn1(q));
       dot_i(q,1) = eph(13,svprn1(q));
       dot_omega(q,1) = eph(17,svprn1(q));
       Cuc(q,1) = eph(8,svprn1(q));
       Cus(q,1) = eph(9,svprn1(q));
       Crc(q,1) = eph(10,svprn1(q));
       Crs(q,1) = eph(11,svprn1(q));
       Cic(q,1) = eph(14,svprn1(q));
       Cis(q,1) = eph(15,svprn1(q));
       Toe(q,1) = eph(18,svprn1(q));
       
%計算衛星的位置即ECEF下的坐標、衛星到基線中點的單位向量 
    tk=t_k-Toe(q,1);
    if (tk>302400)
        tk=tk-604800;
    end
    if (tk<-302400)
        tk=tk+604800;
    end
    M_k(q,1)=M0(q,1)+(sqrt(mu/sqrt_a(q,1)^6)+delta_n(q,1))*tk;    
       %%%%%%%%%%%%% 求偏近點角 E_k %%%%%%%%%%%%% 
          Et_1(q,1)=M_k(q,1);
          t_end=1;  
          while(t_end)
              Et(q,1)=M_k(q,1)+s_e(q,1)*sin(Et_1(q,1));        
              delta_E(q,1)=Et(q,1)-Et_1(q,1);
              Et_1(q,1)=Et(q,1);
              if abs(delta_E(q,1))<=1.0e-6
                 E_k(q,1)=Et(q,1);  
                 t_end=0;
              end 
          end 
       %%%%%%%%%%%%%% 求真近點角 f 的值 %%%%%%%%%%
          %A=(cos(E_k(q,j))-e)/(1-e*cos(E_k(q,j)));          %% f 的余弦
          A=cos(E_k(q,1))-s_e(q,1);               %分母一定是是大于0的數,所以只取分子來做判斷
          %B=(sin(E_k(q,j))*sqrt(1-e^2))/(1-e*cos(E_k(q,j)));%% f 的正弦
          B=sqrt(1-s_e(q,1)^2)*sin(E_k(q,1));

         if (A==0)
             if(B<0)
             f(q,1)=-pi/2;
             else
             f(q,1)=pi/2;
             end       
         else
             f(q,1)=atan(B/A);
             if ((B>=0)&(A<0))
                 f(q,1)=pi+f(q,1);
             end
             if ((B<0)&(A<0))
                 f(q,1)=-pi+f(q,1);
             end
         end
         % 求偏近點角值\真近點角值\平近點角值相互之間的差值 delta_Ef ，delta_EM，delta_fM %   
                  
         % 計算升交點角距u_k,地心距r_k,和軌道傾角i_k ,升交點的經度L_k %
         u_k(q,1)=s_w(q,1)+f(q,1)+Cuc(q,1)*cos(2*(s_w(q,1)+f(q,1)))+Cus(q,1)*sin(2*(s_w(q,1)+f(q,1)));
         % u_k(q,j)=f(q,j);         
         r_k(q,1)=sqrt_a(q,1)^2*(1-s_e(q,1)*cos(E_k(q,1)))+Crc(q,1)*cos(2*(s_w(q,1)+f(q,1)))+Crs(q,1)*sin(2*(s_w(q,1)+f(q,1)));
         % r_k(q,j)=a*(1-e*cos(E_k(q,j)));    
         i_k(q,1)=i0(q,1)+dot_i(q,1)*(t_k-Toe(q,1))+Cic(q,1)*cos(2*(s_w(q,1)+f(q,1)))+Cis(q,1)*sin(2*(s_w(q,1)+f(q,1)));
         % i_k(q,j)=i_0;    
         L_k(q,1)=omega0(q,1)+(dot_omega(q,1)-w_ie)*tk-w_ie*Toe(q,1);
         % L_k(q,j)=sate(j,2)+w_ie*(t_k);%%%%%%%%%%%  jia 還是 jian ？？
         % 計算導航星的位置,在地心坐標系中,ECEF(Earth-Centered Earth-Fixed)坐標系 %
         x_k(q,1)=r_k(q,1)*cos(u_k(q,1))*cos(L_k(q,1))-r_k(q,1)*sin(u_k(q,1))*sin(L_k(q,1))*cos(i_k(q,1));    
         y_k(q,1)=r_k(q,1)*cos(u_k(q,1))*sin(L_k(q,1))+r_k(q,1)*sin(u_k(q,1))*cos(L_k(q,1))*cos(i_k(q,1));
         z_k(q,1)=r_k(q,1)*sin(u_k(q,1))*sin(i_k(q,1));
         
         %計算衛星和測站的初始距離
         R(q,1)=sqrt((x_k(q,1)-xp0)^2+(y_k(q,1)-yp0)^2+(z_k(q,1)-zp0)^2);
         %計算A矩陣
         AA(q,1)=(x_k(q,1)-xp0)/R(q,1);
         AA(q,2)=(y_k(q,1)-yp0)/R(q,1);
         AA(q,3)=(z_k(q,1)-zp0)/R(q,1);
         AA(q,4)=-1;
         %計算矩陣L
         L(q,1)=pse(q,1)-R(q,1);
end

for i=1:3
    delta_T=-inv(AA'*AA)*(AA'*L);
    xp0=xp0+delta_T(1,1);
    yp0=yp0+delta_T(2,1);
    zp0=zp0+delta_T(3,1);
    for q=1:6
        %計算衛星和測站的初始距離
         R(q,1)=sqrt((x_k(q,1)-xp0)^2+(y_k(q,1)-yp0)^2+(z_k(q,1)-zp0)^2);
         %計算A矩陣
         AA(q,1)=(x_k(q,1)-xp0)/R(q,1);
         AA(q,2)=(y_k(q,1)-yp0)/R(q,1);
         AA(q,3)=(z_k(q,1)-zp0)/R(q,1);
         AA(q,4)=-1;
         %計算矩陣L
         L(q,1)=pse(q,1)-R(q,1);
    end    
end

L0=atan(yp0/xp0)*180/pi;
if (yp0>0 & xp0<0) 
    L0=180+L0;
end
if (yp0<0 & xp0<0) 
    L0=-180+L0;
end
B0 = atan(zp0/((1-e_e)^2*sqrt(xp0^2+yp0^2)))*180/pi;
%H0 = (cos(atan(zp0/sqrt(xp0^2+yp0^2)))*sqrt(xp0^2+yp0^2+zp0^2))/cos(B0)-N;

longitude(t) = L0
latitude(t) = B0
%height(t) = H0

end