%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Main Part of The Program, Which asemble diffrent Matlab Function
clc
format long e
clear
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Initial Constant
C=299792458;
F=-4.442807633*10^(-10);
T_amb=20;
P_amb=101;
P_vap=.86;
color=['r','b','c','g','k','m'];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Load Data and Difinition
load project_data.mat
GPS_Time=iono(1);Alpha=iono(2:5);
Beta=iono(6:9);
af=[eph(21,:);eph(20,:);eph(19,:)]';

Toc=[eph(18,:)];
Ttr=GPS_Time-pr./C;
Ttr
ec=[eph(5,:)];
A=[eph(7,:)].^2;
Tgd=[eph(17,:)];

Rho_0=pr;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Compute satellite position and corrected orbit value
[Pos_xyz_Mat,Orbit_parameter]=SV_Ephemeris_Model(iono(1,1),eph);
Pos_SV=Pos_xyz_Mat;   
     E=Orbit_parameter(2,:);
     A=Orbit_parameter(9,:);
     Ec=Orbit_parameter(10,:);
     Inc=Orbit_parameter(7,:);
     Omega=Orbit_parameter(8,:);
     v=Orbit_parameter(3,:);
Dim=size(Pos_SV);
n=Dim(1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%Plot The Geometry%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%plot_Orbit(Orbit_parameter,color)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Initial GPS Position
Xu=0;Yu=0;Zu=0;Pos_Rcv=[Xu Yu Zu];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Start Iteration
%Initialization Parameter
  Iter=1;
  Pos_Rcv_N{Iter}=[Pos_Rcv];
%Constrain for convergence  
  B1=1;
  END_LOOP=100;
while (END_LOOP > B1) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%Compute Satellite Pseudo Range%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Rho=Rho_0+Delta_I+Delta_T+Delta_Rel+Delta_Off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%A.Compute Satellite Offset Clock Error
dTclk_Offset=Error_Satellite_Clock_Offset(af,Ttr,Toc);               %(Sec)
dR_Error_Satellite_Clock_Offset=C*dTclk_Offset;                    %(Meter)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%B.Compute Satellite Relavistic Clock Error
dTclk_Rel=Error_Satellite_Clock_Relavastic(F,ec,A,E,Tgd);            %(Sec)
dR_Error_Satellite_Clock_Rel=C*dTclk_Rel;                          %(Meter)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%C.Compute IonoSphere Error
dT_Ion=Error_Ionospheric_Klobuchar(Pos_Rcv_N{Iter},Pos_SV,Alpha,Beta,GPS_Time);%(Sec)
dR_Error_Ionsphere=C*dT_Ion;                                       %(Meter)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%D.Compute Tropospheric Error
Delta_R_Trop=Error_Tropospheric_Hopfield(T_amb,P_amb,P_vap,Pos_Rcv_N{Iter},Pos_SV);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Compute Pseudo Range
Rho=Rho_0'+dR_Error_Satellite_Clock_Offset'+dR_Error_Satellite_Clock_Rel';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Compute Covariance Matrix,deltax
[G0,Delta_X0,Pos_Rcv_N0,B0]=Gen_G_DX_XYZ_B(Pos_SV,Pos_Rcv_N{Iter},Rho);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Iter=Iter+1
G{Iter}=G0;
Delta_X{Iter}=Delta_X0;
B(Iter)=B0;
Pos_Rcv_N{Iter}=Pos_Rcv_N0;
END_LOOP=norm(Delta_X0(1:3));
end %End of While


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot Resualt
for i=1:Iter-1
    a=Delta_X{i+1}
    dx1(i)=a(1);
    dx2(i)=a(2);
    dx3(i)=a(3);
    de(i)=sqrt(a(1)^2+a(2)^2+a(3)^2);
end
for i=1:Iter
    Pos_R=Pos_Rcv_N{i};
    x1(i)=Pos_R(1);
    x2(i)=Pos_R(2);
    x3(i)=Pos_R(3);

end
a=Pos_Rcv_N{Iter};
%plot3(a(1),a(2),a(3),'*')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%plot Delta X
 figure
 subplot(3,1,1)
 plot(dx1);
grid on
ylabel('\Delta_X');
title('Covariance Solution');
 hold on
subplot(3,1,2)
plot(dx2)
 grid on
ylabel('\Delta_Y')
subplot(3,1,3)
plot(dx3)
grid on
ylabel('\Delta_Z')
xlabel('Iterration')
figure
plot(de)
title('GPS Error Position')
xlabel('Iteration')
ylabel('Norm Error')
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %GPS reciever Position
 figure
 subplot(3,1,1)
 plot(x1)
 title('GPS Reciever Postion')
 grid on
 ylabel('X')
 hold on
 subplot(3,1,2)
 plot(x2)
 grid on
 ylabel('Y')
 subplot(3,1,3)
 plot(x3)
 grid on
 ylabel('_Z')
 xlabel('Iterration')
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure
 plot(B)
 title('GPS Reciever Clock Bias')
 xlabel('Iteration')
 ylabel('GPS Reciever Clock Bias (Sec)')
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% G0=G{Iter};
% GTG=G0'*G0;
% GDOP=sqrt(trace(GTG))
% PDOP=sqrt(GTG(1,1)+GTG(2,2)+GTG(3,3))
% HDOP=sqrt(GTG(1,1)+GTG(2,2))
% VDOP=sqrt(GTG(1,1))
% TDOP=sqrt(GTG(4,4))
% 
%