Project reports
![](http://www.weebly.com/weebly/images/file_icons/gz.png)
gnt08.1.2.zip | |
File Size: | 498 kb |
File Type: | zip |
% Satellite Position By Ephemeris Model
%Function's Inputs:
% GPS_RVC_T:(Sec):
% A:semi-major orbit axis (meter)
% n0: Compute mean motion (rad/s)
% tk:time from ephemeris reference epoch
% n: corrected mean motion
% Dealta_n:correction for mean motion
% M0:mean Anomaly at refernce time
% Mk:Mean anomaly
% Ek: Keplers equation for the eccentric anomaly Ek (rad)
% e: orbit eccentricity
% Toe: Refernce time of ephemeris parameters
% i0: inlination angle at reference time
% Phik:Argument of lattitude
% Omega0: Longitude of Ascending Node
% omega: Argument of perigee (semicicrcle)
% Omegadot: rate of right ascension
% IDOT: rate of inlination angle
% Delta_uk:Argument of lattitude correction
% Delta_rk:Argument of radius correction
% Delta_ik:Argument of inclination correction
% uk:Corrcted argument of lattitude
% rk: corrected radius
% ik:corrected inclination
% xpk:satellite position in orbital plane
% ypk:satellite position in orbital plane
% SEcond Harmonic Perturbation coefiicient
%C_us:Amplitude of the cosine harmonic correction term to the argument
% of lattitude(Rad)
%C_uc:Amplitude of the sine harmonic correction term to the argument
% of lattitude(Rad)
%C_rs:Amplitude of the cosine harmonic correction term to orbit
%radius(meter)
%C_rc:Amplitude of the sine harmonic correction term to orbit
%radius(meter)
%C_is:Amplitude of the cosine harmonic correction term to the angle of
% inclination(Rad)
%C_ic:Amplitude of the sine harmonic correction term to the angle of
% inclination(Rad)
%Function's Outputs:
% xk,yk,zk: Satellite position in ECEF
% Ek:Eccentric Anomaly
% A:semi-major orbit axis
% e: orbit eccentricity
% ik: Corrected Inlination
% Omegak: Corrected longitude of ascending node
% Nuk:True anomaly as a function of the eccentric anomaly
%Function's Constants:
% Mu:WGS-84 value of the earths universal gravitational
% parameter(3.986005*10^14 m4/s2)
% Omega_dote:WGS-84 value of the earths roration rate
% (7.2921151467*10^(-5)rad/s)
%Input:(GPS_RVC_T,[C_rs Dealta_n M0 C_uc e C_us sqrt(A) Toe C_ic Omega0 C_is
% i0 C_rc omega Omegadot IDOT]
%**************************************************************************
function [Pos_xyz_Mat,Orbit_parameter]=SV_Ephemeris_Model(GPS_RVC_T,data);
[Parameter,ST_NO]=size(data);
% Constant Parameter of this function
Mu=3.986005*10^14;
Omega_dote=7.2921151467*10^(-5);
for ST_No=1:ST_NO
%Load Data
A=data(7,ST_No)*data(7,ST_No);
t=GPS_RVC_T;
toe=data(8,ST_No);
Delta_n=data(2,ST_No);
M0=data(3,ST_No);
omega=data(14,ST_No);
C_us=data(6,ST_No);
C_uc=data(4,ST_No);
C_rs=data(1,ST_No);
C_rc=data(13,ST_No);
C_is=data(11,ST_No);C_ic=data(9,ST_No);
i0=data(12,ST_No);
IDOT=data(16,ST_No);
Omega0=data(10,ST_No);
Omegadot=data(15,ST_No);
e=data(5,ST_No);
%**************************************************************************
%**************************************************************************
%Compute Time From Ephemeris Refernce Epoch
%Input:
%t(sec):GPS Sys. Time
%toe(sec):Reference Time Ephemeris
%Output:
%tk(sec):Time from ephemeris reference epoch
tk=t-toe;
%**************************************************************************
%**************************************************************************
%Compute corrected mean motion
%Input:
%A(meter): Semi-major axis
%Mu(meter^3/sec^2): value of Earth's universal gravitational parameters
%tk(sec):Time from Ephemeris Reference Epoch
%Delta_n(semi-circle/sec):Mean Motion Diference From Computed Value
%M0(semi-circles):
%Output:
%n0(Rad/sec):Computed mean motion
%n(semi-circle/sec):Corrected mean motion
%Mk(semi-circle):Mean anomaly
n0=sqrt(Mu/(A^3));
n=n0/pi+Delta_n;
Mk=M0+n*tk;
%**************************************************************************
%**************************************************************************
%Solve Kepler Eq. with Initial value Ek0=Mk
%Use Fzero Function to solve Kepler Eq.
%The fzero command is an M-file. The algorithm, which was originated by
%T. Dekker, uses a combination of bisection, secant, and inverse quadratic
%interpolation methods. An Algol 60 version, with some improvements, is
%given in [Brent, R., Algorithms for Minimization Without Derivatives,
%Prentice-Hall, 1973.]
%Input:
%Mk(semicircle): Mean Anomaly
%e: Eccentricity
%Output:
%Ek(Rad): Eccentric Anomaly
Ek=fzero(@(x) Kepler_Eq(x,Mk*pi,e),Mk*pi);
%**************************************************************************
%**************************************************************************
%Compute True Anomaly as a Function of Eccentric Anomaly
%Input:
%Ek(Rad): Eccentric Anomaly
%e : Eccentricity
%Output:
%nuk(Rad): True Anomaly
Sin_nuk=sqrt(1-e^2)*sin(Ek)/(1-e*cos(Ek));
Cos_nuk=(cos(Ek)-e)/(1-e*cos(Ek));
nuk=atan2(Sin_nuk,Cos_nuk);
if nuk<0
nuk=nuk+2*pi;
end
%**************************************************************************
%**************************************************************************
%Compute Argumet of Lattitude and Correction
%Input:
%nuk(Rad): True Anomaly
%omega(semicircle): Argument of perigee
%Output:
%Phik(Rad): Argument of lattitude
Phik=nuk+omega*pi;
%**************************************************************************
%**************************************************************************
%Compute Argumet of Lattitude,Radious and Inclination Correction
%SEcond Harmonic Perturbation
%Input:
%Phik(Rad): Argument of lattitude
%C_us:Amplitude of the cosine harmonic correction term to the argument
% of lattitude(Rad)
%C_uc:Amplitude of the sine harmonic correction term to the argument
% of lattitude(Rad)
%C_rs:Amplitude of the cosine harmonic correction term to orbit
%radius(meter)
%C_rc:Amplitude of the sine harmonic correction term to orbit
%radius(meter)
%C_is:Amplitude of the cosine harmonic correction term to the angle of
% inclination(Rad)
%C_ic:Amplitude of the sine harmonic correction term to the angle of
% inclination(Rad)
%Output:
% Delta_uk(Rad):Argument of lattitude correction
% Delta_rk(meter):Argument of radius correction
% Delta_ik(Rad):Argument of inclination correction
Delta_uk=C_us*sin(2*Phik)+C_uc*cos(2*Phik);
Delta_rk=C_rs*sin(2*Phik)+C_rc*cos(2*Phik);
Delta_ik=C_is*sin(2*Phik)+C_ic*cos(2*Phik);
%**************************************************************************
%**************************************************************************
%Compute Corrected Value of Lattitude,Radious,Inclination and Ascendong
%node
%Input:
% Phik(Rad): Argument of lattitude
% A(meter): Semi-major axis
% Ek(Rad): Eccentric Anomaly
% e: Eccentricity
% i0(semi-circle): inlination angle at reference time
% Omega0(semi-circle): Refernce Longitude of Ascending Node
% Omegadot(semi-circle/sec): rate of right ascension
% Omega_dote(rad/sec):WGS 84 value of the earth's rotation rate
% IDOT(semi-circle/sec): rate of inlination angle
% Delta_uk(Rad):Argument of lattitude correction
% Delta_rk(meter):Argument of radius correction
% Delta_ik(Rad):Argument of inclination correction
%Output:
% uk(Rad):Corrcted argument of lattitude
% rk(meter): corrected radius
% ik(Rad):corrected inclination
% Omegak(Rad):Corrected longitude of ascending node.
uk=Phik+Delta_uk; %Latitude
rk=A*(1-e*cos(Ek))+Delta_rk; %Radious
ik=i0*pi+Delta_ik+IDOT*tk*pi; %Inclination
Omegak=Omega0*pi+(Omegadot*pi-Omega_dote)*tk-Omega_dote*toe;
%**************************************************************************
%**************************************************************************
%Compute satellite vehicle position
%Satellite position in orbital plane
x_Perk=rk*cos(uk);
y_Perk=rk*sin(uk);
%Satellite Position in ECEF
xk=x_Perk*cos(Omegak)-y_Perk*cos(ik)*sin(Omegak);
yk=x_Perk*sin(Omegak)+y_Perk*cos(ik)*cos(Omegak);
zk=y_Perk*sin(ik);
%Output:
% xk,yk,zk(meter): Satellite Position in ECEF
% Mk(semicircle): Mean Anomaly
% Ek(Rad): Eccentric Anomaly
% nuk(Rad): True Anomaly
% Phik(Rad): Argument of lattitude
% uk(Rad):Corrcted argument of lattitude
% rk(meter): corrected radius
% ik(Rad):corrected inclination
% Omegak(Rad):Corrected longitude of ascending node.
% A(meter):semi-major orbit axis
% e: orbit eccentricity
Pos_xyz=[xk yk zk Mk Ek nuk Phik uk rk ik Omegak A e];
Pos_xyz_Mat(ST_No,1)=Pos_xyz(1);Pos_xyz_Mat(ST_No,2)=Pos_xyz(2);Pos_xyz_Mat(ST_No,3)=Pos_xyz(3);
for j=1:10
Orbit_parameter(j,ST_No)=Pos_xyz(j+3);
end
end
%Function's Inputs:
% GPS_RVC_T:(Sec):
% A:semi-major orbit axis (meter)
% n0: Compute mean motion (rad/s)
% tk:time from ephemeris reference epoch
% n: corrected mean motion
% Dealta_n:correction for mean motion
% M0:mean Anomaly at refernce time
% Mk:Mean anomaly
% Ek: Keplers equation for the eccentric anomaly Ek (rad)
% e: orbit eccentricity
% Toe: Refernce time of ephemeris parameters
% i0: inlination angle at reference time
% Phik:Argument of lattitude
% Omega0: Longitude of Ascending Node
% omega: Argument of perigee (semicicrcle)
% Omegadot: rate of right ascension
% IDOT: rate of inlination angle
% Delta_uk:Argument of lattitude correction
% Delta_rk:Argument of radius correction
% Delta_ik:Argument of inclination correction
% uk:Corrcted argument of lattitude
% rk: corrected radius
% ik:corrected inclination
% xpk:satellite position in orbital plane
% ypk:satellite position in orbital plane
% SEcond Harmonic Perturbation coefiicient
%C_us:Amplitude of the cosine harmonic correction term to the argument
% of lattitude(Rad)
%C_uc:Amplitude of the sine harmonic correction term to the argument
% of lattitude(Rad)
%C_rs:Amplitude of the cosine harmonic correction term to orbit
%radius(meter)
%C_rc:Amplitude of the sine harmonic correction term to orbit
%radius(meter)
%C_is:Amplitude of the cosine harmonic correction term to the angle of
% inclination(Rad)
%C_ic:Amplitude of the sine harmonic correction term to the angle of
% inclination(Rad)
%Function's Outputs:
% xk,yk,zk: Satellite position in ECEF
% Ek:Eccentric Anomaly
% A:semi-major orbit axis
% e: orbit eccentricity
% ik: Corrected Inlination
% Omegak: Corrected longitude of ascending node
% Nuk:True anomaly as a function of the eccentric anomaly
%Function's Constants:
% Mu:WGS-84 value of the earths universal gravitational
% parameter(3.986005*10^14 m4/s2)
% Omega_dote:WGS-84 value of the earths roration rate
% (7.2921151467*10^(-5)rad/s)
%Input:(GPS_RVC_T,[C_rs Dealta_n M0 C_uc e C_us sqrt(A) Toe C_ic Omega0 C_is
% i0 C_rc omega Omegadot IDOT]
%**************************************************************************
function [Pos_xyz_Mat,Orbit_parameter]=SV_Ephemeris_Model(GPS_RVC_T,data);
[Parameter,ST_NO]=size(data);
% Constant Parameter of this function
Mu=3.986005*10^14;
Omega_dote=7.2921151467*10^(-5);
for ST_No=1:ST_NO
%Load Data
A=data(7,ST_No)*data(7,ST_No);
t=GPS_RVC_T;
toe=data(8,ST_No);
Delta_n=data(2,ST_No);
M0=data(3,ST_No);
omega=data(14,ST_No);
C_us=data(6,ST_No);
C_uc=data(4,ST_No);
C_rs=data(1,ST_No);
C_rc=data(13,ST_No);
C_is=data(11,ST_No);C_ic=data(9,ST_No);
i0=data(12,ST_No);
IDOT=data(16,ST_No);
Omega0=data(10,ST_No);
Omegadot=data(15,ST_No);
e=data(5,ST_No);
%**************************************************************************
%**************************************************************************
%Compute Time From Ephemeris Refernce Epoch
%Input:
%t(sec):GPS Sys. Time
%toe(sec):Reference Time Ephemeris
%Output:
%tk(sec):Time from ephemeris reference epoch
tk=t-toe;
%**************************************************************************
%**************************************************************************
%Compute corrected mean motion
%Input:
%A(meter): Semi-major axis
%Mu(meter^3/sec^2): value of Earth's universal gravitational parameters
%tk(sec):Time from Ephemeris Reference Epoch
%Delta_n(semi-circle/sec):Mean Motion Diference From Computed Value
%M0(semi-circles):
%Output:
%n0(Rad/sec):Computed mean motion
%n(semi-circle/sec):Corrected mean motion
%Mk(semi-circle):Mean anomaly
n0=sqrt(Mu/(A^3));
n=n0/pi+Delta_n;
Mk=M0+n*tk;
%**************************************************************************
%**************************************************************************
%Solve Kepler Eq. with Initial value Ek0=Mk
%Use Fzero Function to solve Kepler Eq.
%The fzero command is an M-file. The algorithm, which was originated by
%T. Dekker, uses a combination of bisection, secant, and inverse quadratic
%interpolation methods. An Algol 60 version, with some improvements, is
%given in [Brent, R., Algorithms for Minimization Without Derivatives,
%Prentice-Hall, 1973.]
%Input:
%Mk(semicircle): Mean Anomaly
%e: Eccentricity
%Output:
%Ek(Rad): Eccentric Anomaly
Ek=fzero(@(x) Kepler_Eq(x,Mk*pi,e),Mk*pi);
%**************************************************************************
%**************************************************************************
%Compute True Anomaly as a Function of Eccentric Anomaly
%Input:
%Ek(Rad): Eccentric Anomaly
%e : Eccentricity
%Output:
%nuk(Rad): True Anomaly
Sin_nuk=sqrt(1-e^2)*sin(Ek)/(1-e*cos(Ek));
Cos_nuk=(cos(Ek)-e)/(1-e*cos(Ek));
nuk=atan2(Sin_nuk,Cos_nuk);
if nuk<0
nuk=nuk+2*pi;
end
%**************************************************************************
%**************************************************************************
%Compute Argumet of Lattitude and Correction
%Input:
%nuk(Rad): True Anomaly
%omega(semicircle): Argument of perigee
%Output:
%Phik(Rad): Argument of lattitude
Phik=nuk+omega*pi;
%**************************************************************************
%**************************************************************************
%Compute Argumet of Lattitude,Radious and Inclination Correction
%SEcond Harmonic Perturbation
%Input:
%Phik(Rad): Argument of lattitude
%C_us:Amplitude of the cosine harmonic correction term to the argument
% of lattitude(Rad)
%C_uc:Amplitude of the sine harmonic correction term to the argument
% of lattitude(Rad)
%C_rs:Amplitude of the cosine harmonic correction term to orbit
%radius(meter)
%C_rc:Amplitude of the sine harmonic correction term to orbit
%radius(meter)
%C_is:Amplitude of the cosine harmonic correction term to the angle of
% inclination(Rad)
%C_ic:Amplitude of the sine harmonic correction term to the angle of
% inclination(Rad)
%Output:
% Delta_uk(Rad):Argument of lattitude correction
% Delta_rk(meter):Argument of radius correction
% Delta_ik(Rad):Argument of inclination correction
Delta_uk=C_us*sin(2*Phik)+C_uc*cos(2*Phik);
Delta_rk=C_rs*sin(2*Phik)+C_rc*cos(2*Phik);
Delta_ik=C_is*sin(2*Phik)+C_ic*cos(2*Phik);
%**************************************************************************
%**************************************************************************
%Compute Corrected Value of Lattitude,Radious,Inclination and Ascendong
%node
%Input:
% Phik(Rad): Argument of lattitude
% A(meter): Semi-major axis
% Ek(Rad): Eccentric Anomaly
% e: Eccentricity
% i0(semi-circle): inlination angle at reference time
% Omega0(semi-circle): Refernce Longitude of Ascending Node
% Omegadot(semi-circle/sec): rate of right ascension
% Omega_dote(rad/sec):WGS 84 value of the earth's rotation rate
% IDOT(semi-circle/sec): rate of inlination angle
% Delta_uk(Rad):Argument of lattitude correction
% Delta_rk(meter):Argument of radius correction
% Delta_ik(Rad):Argument of inclination correction
%Output:
% uk(Rad):Corrcted argument of lattitude
% rk(meter): corrected radius
% ik(Rad):corrected inclination
% Omegak(Rad):Corrected longitude of ascending node.
uk=Phik+Delta_uk; %Latitude
rk=A*(1-e*cos(Ek))+Delta_rk; %Radious
ik=i0*pi+Delta_ik+IDOT*tk*pi; %Inclination
Omegak=Omega0*pi+(Omegadot*pi-Omega_dote)*tk-Omega_dote*toe;
%**************************************************************************
%**************************************************************************
%Compute satellite vehicle position
%Satellite position in orbital plane
x_Perk=rk*cos(uk);
y_Perk=rk*sin(uk);
%Satellite Position in ECEF
xk=x_Perk*cos(Omegak)-y_Perk*cos(ik)*sin(Omegak);
yk=x_Perk*sin(Omegak)+y_Perk*cos(ik)*cos(Omegak);
zk=y_Perk*sin(ik);
%Output:
% xk,yk,zk(meter): Satellite Position in ECEF
% Mk(semicircle): Mean Anomaly
% Ek(Rad): Eccentric Anomaly
% nuk(Rad): True Anomaly
% Phik(Rad): Argument of lattitude
% uk(Rad):Corrcted argument of lattitude
% rk(meter): corrected radius
% ik(Rad):corrected inclination
% Omegak(Rad):Corrected longitude of ascending node.
% A(meter):semi-major orbit axis
% e: orbit eccentricity
Pos_xyz=[xk yk zk Mk Ek nuk Phik uk rk ik Omegak A e];
Pos_xyz_Mat(ST_No,1)=Pos_xyz(1);Pos_xyz_Mat(ST_No,2)=Pos_xyz(2);Pos_xyz_Mat(ST_No,3)=Pos_xyz(3);
for j=1:10
Orbit_parameter(j,ST_No)=Pos_xyz(j+3);
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%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;
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))
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%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;
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))
%
%
clc
format long e
clear all
close all
C=299792458; %
F=-4.442807633*10^(-10); %?
T_amb=20;
P_amb=101; %?
P_vap=.86; %?
color=['r','b','c','g','k','m']; % K?
load project_data.mat %data?
GPS_Time=iono(1); %iono(1)?
Alpha=iono(2:5); %values?
Beta=iono(6:9); %valus?
af=[eph(21,:);eph(20,:);eph(19,:)]'; %eph? af?
Toc=[eph(18,:)]; %Toc?
Ttr=GPS_Time-pr./C;%pr? propogation
ec=[eph(5,:)]; %ec?
A=[eph(7,:)].^2;%?
Tgd=[eph(17,:)];%?
Rho_0=pr; %
[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_Orbit(Orbit_parameter,color)
Xu=0;
Yu=0;
Zu=0;
Pos_Rcv=[Xu Yu Zu];
Iter=1;
Pos_Rcv_N{Iter}=[Pos_Rcv];
B1=1;
END_LOOP=100;
while (END_LOOP > B1)
%Rho=Rho_0+Delta_I+Delta_T+Delta_Rel+Delta_Off
dTclk_Offset=Error_Satellite_Clock_Offset(af,Ttr,Toc); %(Sec)
dR_Error_Satellite_Clock_Offset=C*dTclk_Offset; %(Meter)
dTclk_Rel=Error_Satellite_Clock_Relavastic(F,ec,A,E,Tgd); %(Sec)
dR_Error_Satellite_Clock_Rel=C*dTclk_Rel; %(Meter)
dT_Ion=Error_Ionospheric_Klobuchar(Pos_Rcv_N{Iter},Pos_SV,Alpha,Beta,GPS_Time);%(Sec)
dR_Error_Ionsphere=C*dT_Ion; %(Meter)
Delta_R_Trop=Error_Tropospheric_Hopfield(T_amb,P_amb,P_vap,Pos_Rcv_N{Iter},Pos_SV);
Rho=Rho_0'+dR_Error_Satellite_Clock_Offset'+dR_Error_Satellite_Clock_Rel';
[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
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),'*')
format long e
clear all
close all
C=299792458; %
F=-4.442807633*10^(-10); %?
T_amb=20;
P_amb=101; %?
P_vap=.86; %?
color=['r','b','c','g','k','m']; % K?
load project_data.mat %data?
GPS_Time=iono(1); %iono(1)?
Alpha=iono(2:5); %values?
Beta=iono(6:9); %valus?
af=[eph(21,:);eph(20,:);eph(19,:)]'; %eph? af?
Toc=[eph(18,:)]; %Toc?
Ttr=GPS_Time-pr./C;%pr? propogation
ec=[eph(5,:)]; %ec?
A=[eph(7,:)].^2;%?
Tgd=[eph(17,:)];%?
Rho_0=pr; %
[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_Orbit(Orbit_parameter,color)
Xu=0;
Yu=0;
Zu=0;
Pos_Rcv=[Xu Yu Zu];
Iter=1;
Pos_Rcv_N{Iter}=[Pos_Rcv];
B1=1;
END_LOOP=100;
while (END_LOOP > B1)
%Rho=Rho_0+Delta_I+Delta_T+Delta_Rel+Delta_Off
dTclk_Offset=Error_Satellite_Clock_Offset(af,Ttr,Toc); %(Sec)
dR_Error_Satellite_Clock_Offset=C*dTclk_Offset; %(Meter)
dTclk_Rel=Error_Satellite_Clock_Relavastic(F,ec,A,E,Tgd); %(Sec)
dR_Error_Satellite_Clock_Rel=C*dTclk_Rel; %(Meter)
dT_Ion=Error_Ionospheric_Klobuchar(Pos_Rcv_N{Iter},Pos_SV,Alpha,Beta,GPS_Time);%(Sec)
dR_Error_Ionsphere=C*dT_Ion; %(Meter)
Delta_R_Trop=Error_Tropospheric_Hopfield(T_amb,P_amb,P_vap,Pos_Rcv_N{Iter},Pos_SV);
Rho=Rho_0'+dR_Error_Satellite_Clock_Offset'+dR_Error_Satellite_Clock_Rel';
[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
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),'*')