www.pudn.com > final-GPS.rar > main.m, change:2008-04-07,size:5492b


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%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)) 
%  
%