衛(wèi)星導(dǎo)航GPS典型例題編程報(bào)告

1、GPS Matlab編程報(bào)告一、通過星歷計(jì)算衛(wèi)星坐標(biāo)（修正）1、 算法流程（1） 計(jì)算GPS衛(wèi)星運(yùn)行的平均速度 （2） 計(jì)算歸化時(shí)間（3） 計(jì)算計(jì)算觀測歷元的平近點(diǎn)角（4） 計(jì)算偏近點(diǎn)角利用不動(dòng)點(diǎn)迭代法求解此方程： 得出不動(dòng)點(diǎn)迭代法的迭代公式為：（5） 計(jì)算衛(wèi)星的地心矢徑（6） 計(jì)算真近點(diǎn)角（7） 計(jì)算升交點(diǎn)角距（8） 計(jì)算攝動(dòng)改正項(xiàng)：（9） 計(jì)算經(jīng)過攝動(dòng)改正的升交點(diǎn)角距，衛(wèi)星矢徑，和軌道面傾角（10） 計(jì)算觀測歷元的升交點(diǎn)經(jīng)度（11） 計(jì)算衛(wèi)星在軌道平面坐標(biāo)系中的位置（12） 計(jì)算衛(wèi)星在地固坐標(biāo)系下的坐標(biāo)2、 Matlab源程序%本程序用于通過星歷計(jì)算衛(wèi)星坐標(biāo)（修正）%部分星歷數(shù)據(jù)未用到cl

2、ear;%-t時(shí)刻，衛(wèi)星星歷數(shù)據(jù)- t=1.728128741751984e+005; %當(dāng)前時(shí)間（接收到衛(wèi)星信號(hào)時(shí)間） Omega0= -3.1122016819; %按參考時(shí)間計(jì)算的升交點(diǎn)經(jīng)度 I0=0.3*pi; %非GEO衛(wèi)星 %I0= 0.0879969068; %GEO衛(wèi)星 SqrtA= 6493.27499625; %長半軸平方根 Ecc= 0.01995786; %偏心率 Small_Omega= -1.7409228484; %近地點(diǎn)角距 Mu0= 0.0005884201; %參考時(shí)間的平近點(diǎn)角 Delta_n= -2.4123e-009; %衛(wèi)星平均運(yùn)動(dòng)速度與計(jì)算值之差

3、I_Dot= -3.4333e-010; %軌道傾角變化率 Omega_dot= 3.47676e-009; %升交點(diǎn)經(jīng)度變化率 C_uc= 4.692e-006; %緯度幅角的余弦調(diào)和改正項(xiàng)的振幅 C_us= -1.10036e-005; %緯度幅角的正弦調(diào)和改正項(xiàng)的振幅 C_ic= 9.01e-008; %軌道傾角的余弦調(diào)和改正項(xiàng)的振幅 C_is= 2.01e-008; %軌道傾角的正弦調(diào)和改正項(xiàng)的振幅 C_rc= 325.73368; %軌道半徑的余弦調(diào)和改正項(xiàng)的振幅 C_rs= 146.91074; %軌道半徑的正弦調(diào)和改正項(xiàng)的振幅 Toe= 172800; %星歷參考時(shí)間IODC=

4、10; %鐘差數(shù)據(jù)齡期URAI= 0; %用戶距離精度標(biāo)志IODE= 10; %星歷數(shù)據(jù)齡期Toc = 172800; %本時(shí)段鐘差參數(shù)參考時(shí)間a0 = -2.72893e-005; %衛(wèi)星鐘差改正0階多項(xiàng)式系數(shù) a1 = -6.7531e-014; %衛(wèi)星鐘差改正1階多項(xiàng)式系數(shù)a2 = 5.787e-018; %衛(wèi)星鐘差改正2階多項(xiàng)式系數(shù)%-定義常量-c=2.99792458e8; %光速omegae=7.2921151467e-5; %地球自轉(zhuǎn)角速度 mu=3.986004418e14; %地球引力常數(shù)GM%-1、計(jì)算GPS衛(wèi)星運(yùn)行的平均速度n-a=SqrtA*SqrtA;n=sqrt(m

5、u/(a3)+Delta_n;%-2、計(jì)算歸化時(shí)間Delta_t-Delta_t=t-Toe;%-3、計(jì)算觀測歷元t的平近點(diǎn)角M-M=Mu0+n*Delta_t;%-4、計(jì)算偏近點(diǎn)角E-eps=1e-20;E=M;tol=1;while (tol>eps) %不動(dòng)點(diǎn)迭代法 E0=E; E=M+(Ecc)*sin(E0); tol=abs(E-E0);end%-5、計(jì)算衛(wèi)星的地心矢徑r0-r0=a*(1-Ecc*cos(E);%-6、計(jì)算真近點(diǎn)角f-%f=2*atan(sqrt(1+Ecc)/(1-Ecc)*tan(E/2);f=atan(sqrt(1-Ecc2)*sin(E)/(cos(

6、E)-Ecc);%-7、計(jì)算升交點(diǎn)角距Phi0-Phi0=Small_Omega+f;%-8、計(jì)算攝動(dòng)改正項(xiàng)：Delta_u,Delta_r,Delta_i-Delta_u=C_us*sin(2*Phi0)+C_uc*cos(2*Phi0);Delta_r=C_rs*sin(2*Phi0)+C_rc*cos(2*Phi0);Delta_i=C_is*sin(2*Phi0)+C_ic*cos(2*Phi0);%-9、計(jì)算經(jīng)過攝動(dòng)改正的升交點(diǎn)角距Phi，衛(wèi)星矢徑r，和軌道面傾角I-Phi=Phi0+Delta_u;r=r0+Delta_r;I=I0+Delta_i+I_Dot*Delta_t;%-

7、10、計(jì)算觀測歷元t的升交點(diǎn)經(jīng)度Omegak-Omegak=Omega0+(Omega_dot-omegae)*Delta_t-omegae*Toe;%-11、計(jì)算衛(wèi)星在軌道平面坐標(biāo)系中的位置-x0=r*cos(Phi);y0=r*sin(Phi);z0=0;%-12、計(jì)算衛(wèi)星在地固坐標(biāo)系下的坐標(biāo)-x=x0*cos(Omegak)-y0*cos(I)*sin(Omegak);y=x0*sin(Omegak)+y0*cos(I)*cos(Omegak);z=y0*sin(I)+z0*cos(I);%-輸出衛(wèi)星坐標(biāo)-fprintf ('（修正后）衛(wèi)星在地固坐標(biāo)系中的坐標(biāo)：n X=%.10f

8、 Y=%.10f Z=%.10fn',x,y,z);%老師給的結(jié)果：衛(wèi)星位置：X=7073881.4181256806 Y=23901970.8378255780 Z=-32955601.5025106560X=7073881.4181256806;Y=23901970.837825578;Z=-32955601.5025106560;%修正后的，非GEOfprintf ('與老師的結(jié)果的偏差為：n D_X=%.10f D_Y=%.10f D_Z=%.10fn',x-X,y-Y,z-Z);3、 程序運(yùn)行結(jié)果（修正后）衛(wèi)星在地固坐標(biāo)系中的坐標(biāo)： X=7073887.514

9、4389411 Y=23901969.0496121940 Z=-32955601.4908931700與老師的結(jié)果的偏差為： D_X=6.0963132605 D_Y=-1.7882133834 D_Z=0.0116174854二、已知4顆衛(wèi)星坐標(biāo)及測得的偽距（已改正）求接收機(jī)位置1、 原理及算法不考慮電離層延遲和對流層延遲的因素時(shí)，由4顆衛(wèi)星在地心坐標(biāo)系中的坐標(biāo)及對應(yīng)的偽距即可計(jì)算出接收機(jī)的位置，按如下四元二次方程組求解四個(gè)未知數(shù)：其中，為已改正的偽距，為衛(wèi)星與接收機(jī)之間實(shí)際的距離，分別為4顆衛(wèi)星的坐標(biāo)，為接收機(jī)在地心坐標(biāo)系中的坐標(biāo)，為接收機(jī)時(shí)鐘與衛(wèi)星時(shí)鐘的偏差值。采用Newton迭代法求

10、解此四元二次非線性方程組，其算法為：將上述方程組寫成如下形式四元方程組：記向量，則上式化為：若給出此方程組的一個(gè)初值，將函數(shù)的分量在處Taylor展開，并取線性部分，則可表示為：再由，可認(rèn)為：（1）其中Jacobi矩陣為：則（1）方程組的解為：（2）利用（2）式進(jìn)行迭代運(yùn)算即可解出滿足一定精度要求的近似解。若考慮地球自轉(zhuǎn)影響，需要確定地球的自轉(zhuǎn)角速度以及發(fā)射信號(hào)瞬時(shí)到接收信號(hào)瞬時(shí)經(jīng)歷的時(shí)間，從而計(jì)算出衛(wèi)星信號(hào)傳輸時(shí)間內(nèi)地球自轉(zhuǎn)過的角度，然后利用旋轉(zhuǎn)矩陣將上述計(jì)算得到的坐標(biāo)值做一旋轉(zhuǎn)變換，得出考慮地球自轉(zhuǎn)影響時(shí)接收機(jī)位置，其旋轉(zhuǎn)操作如下：其中衛(wèi)星信號(hào)傳輸總時(shí)間可由最大偽距除以光速得到：，（為光速

11、）。2、 Matlab源程序function Receiver_Position%本程序求解已知4顆衛(wèi)星坐標(biāo)及測得的偽距時(shí)接收機(jī)的位置坐標(biāo)X Y Z，需要求解四元二次非線性方程組%利用Newton法求解此非線性方程組clear;tic;c=2.99792458e8; %光速omegae=7.2921151467e-5;%地球自轉(zhuǎn)角速度x0=0;0;0;0; %取初始值tol=1.0e-6; %計(jì)算精度x1=x0-inv(df(x0)*fc(x0); %計(jì)算第一個(gè)迭代值n=1;while(norm(x1-x0)>=tol)&&n<108) %牛頓迭代計(jì)算 x0=x1;

12、 x1=x0-inv(df(x0)*fc(x0); %迭代公式 n=n+1;endT=toc;fprintf ('接收機(jī)坐標(biāo)為：X=%.10f Y=%.10f Z=%.10fn',x1(1),x1(2),x1(3);%老師的計(jì)算結(jié)果： x=1725670.7674292959 y=-2116958.3717429629 z=3129817.7967605507x=1725670.7674292959;y=-2116958.3717429629;z=3129817.7967605507;fprintf ('與老師的結(jié)果的偏差為：D_X=%.10f D_Y=%.10f D_

13、Z=%.10fn',x1(1)-x,x1(2)-y,x1(3)-z);fprintf ('迭代次數(shù)為：n=%in',n);fprintf ('計(jì)算時(shí)間為：T=%f sn',T);fprintf ('計(jì)算精度為：tol=%fnn',tol);%考慮地球自轉(zhuǎn)因素R=2.4310764064e+007,2.2914600784e+007,2.0628809405e+007,2.3422377972e+007; %偽距D_t=max(R)/c; %取最大的偽距除以光速得到衛(wèi)星信號(hào)傳輸時(shí)間D_phi=omegae*D_t; %發(fā)射信號(hào)瞬時(shí)到接收信號(hào)

14、瞬時(shí)，地球偏轉(zhuǎn)的角度I=cos(D_ phi), sin(D_ phi), 0; -sin(D_ phi), cos(D_ phi) , 0; 0, 0, 1; %旋轉(zhuǎn)矩陣xr=I*x1(1);x1(2);x1(3); %旋轉(zhuǎn)后坐標(biāo)%老師的計(jì)算結(jié)果： x=1725657.493646610 y=-2116969.3298358698 z=3129817.4224583404x=1725657.4936462610;y=-2116969.3298358698;z=3129817.4224583404;fprintf ('考慮地球自轉(zhuǎn)因素時(shí)：n接收機(jī)坐標(biāo)為：X=%.10f Y=%.10f

15、Z=%.10fn',xr(1),xr(2),xr(3);fprintf ('與老師的結(jié)果的偏差為：D_X=%.10f D_Y=%.10f D_Z=%.10fn',xr(1)-x,xr(2)-y,xr(3)-z);end%-原方程組-function Y=fc(X)c=2.99792458e8; %光速%4顆衛(wèi)星在WGS-84坐標(biāo)系中的的X坐標(biāo)，Y坐標(biāo)，Z坐標(biāo)以及接收機(jī)測得的對應(yīng)改正后的偽距x=1.4832308660e+007,-1.5799854050e+007,1.984818910e+006,-1.2480273190e+007'y=2.046671589

16、0e+007,-1.3301129170e+007,-1.1867672960e+007,-2.3382560530e+007'z=7.428634750e+006,1.7133838240e+007,2.3716920130e+007,3.278472680e+006'R=2.4310764064e+007,2.2914600784e+007,2.0628809405e+007,2.3422377972e+007'Y=(x-X(1).2+(y-X(2).2+(z-X(3).2-(R-c*X(4).2;%原方程組（矩陣表示）end%-Jacobi矩陣-function

17、Y=df(X)c=2.99792458e8; %光速%4顆衛(wèi)星在WGS-84坐標(biāo)系中的的X坐標(biāo)，Y坐標(biāo)，Z坐標(biāo)以及接收機(jī)測得的對應(yīng)改正后的偽距x=1.4832308660e+007,-1.5799854050e+007,1.984818910e+006,-1.2480273190e+007'y=2.0466715890e+007,-1.3301129170e+007,-1.1867672960e+007,-2.3382560530e+007'z=7.428634750e+006,1.7133838240e+007,2.3716920130e+007,3.278472680e+0

18、06'R=2.4310764064e+007,2.2914600784e+007,2.0628809405e+007,2.3422377972e+007'%Y=-2*(x-X(1)-2*(y-X(2)-2*(z-X(3)+2*c*(R-c*X(4);Y=-2*(x(1)-X(1),-2*(y(1)-X(2),-2*(z(1)-X(3),2*c*(R(1)-c*X(4); -2*(x(2)-X(1),-2*(y(2)-X(2),-2*(z(2)-X(3),2*c*(R(2)-c*X(4); -2*(x(3)-X(1),-2*(y(3)-X(2),-2*(z(3)-X(3),2*c*(R(3)-c*X(4); -2*(x(4)-X(1),-2*(y(4)-X(2),-2*(z(4)-X(3),2*c*(R(4)-c*X(4);%Jacobi矩陣End3、 程序運(yùn)行結(jié)果接收機(jī)坐標(biāo)為：X=1725670.7674292948 Y