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

1、GPSMatlab編程報(bào)告一、通過星歷計(jì)算衛(wèi)星坐標(biāo)(修正)1、算法流程n=0,a=a3(1)計(jì)算GPS衛(wèi)星運(yùn)行的平均速度nn=n+An0(2)計(jì)算歸化時(shí)間AtAt=ttoe計(jì)算計(jì)算觀測(cè)歷元t的平近點(diǎn)角MM=M+nAt0計(jì)算偏近點(diǎn)角E利用不動(dòng)點(diǎn)迭代法求解此方程：E=M+esinE得出E不動(dòng)點(diǎn)迭代法的迭代公式為：x=p(x)k=0,1,2.k+ik計(jì)算衛(wèi)星的地心矢徑r0r=a(1-ecosE)06)計(jì)算真近點(diǎn)角ff=2*arctan(fE)tanHi-e2丿)計(jì)算升交點(diǎn)角距。o%=+f(8)計(jì)算攝動(dòng)改正項(xiàng)：3,3,3uri3=Csin29+Ccos2申uus0uc03=Csin29+Ccos29r

2、rs0rc03=Csin29+Ccos29iis0ic0(9)計(jì)算經(jīng)過攝動(dòng)改正的升交點(diǎn)角距9，衛(wèi)星矢徑r，和軌道面傾角i申=申+50ur=r+50ri=i+5+iAt0計(jì)算觀測(cè)歷元t的升交點(diǎn)經(jīng)度o,kO=O+(O-co)At-cotk0ieieoe計(jì)算衛(wèi)星在軌道平面坐標(biāo)系中的位置(x,y,z)000(x)cosQ0y=rsin申00丿012)計(jì)算衛(wèi)星在地固坐標(biāo)系下的坐標(biāo)(x,y,z)%當(dāng)前時(shí)間(接收到衛(wèi)星信號(hào)時(shí)間)%按參考時(shí)間計(jì)算的升交點(diǎn)經(jīng)度%非GEO衛(wèi)星%GEO衛(wèi)星%長半軸平方根%偏心率%近地點(diǎn)角距%參考時(shí)間的平近點(diǎn)角%衛(wèi)星平均運(yùn)動(dòng)速度與計(jì)算值之差%軌道傾角變化率%升交點(diǎn)經(jīng)度變化率%緯度幅角

3、的余弦調(diào)和改正項(xiàng)的振幅%緯度幅角的正弦調(diào)和改正項(xiàng)的振幅%軌道傾角的余弦調(diào)和改正項(xiàng)的振幅%軌道傾角的正弦調(diào)和改正項(xiàng)的振幅%軌道半徑的余弦調(diào)和改正項(xiàng)的振幅%軌道半徑的正弦調(diào)和改正項(xiàng)的振幅(xrx)rx-cosO-y-cosi-sinO00k0ky=R(-Q)R(i)y=x-sinO+y-cosi-cosOzkx00k0keps)%不動(dòng)點(diǎn)迭代法E0=E;E=M+(Ecc)*sin(E0);tol=abs(E-E0);end%5、計(jì)算衛(wèi)星的地心矢徑r0r0=a*(1-Ecc*cos(E);%6、計(jì)算真近點(diǎn)角f%f=2*atan(sqrt(1+Ecc)/(1-Ecc)*tan(E/2);f=atan(s

4、qrt(l-Ec2)*sin(E)/(cos(E)-Ecc);%7、計(jì)算升交點(diǎn)角距Phi0Phi0=Small_Omega+f;%8、計(jì)算攝動(dòng)改正項(xiàng)：Delta_u,Delta_r,Delta_iDelta_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,和軌道面傾角IPhi=Phi0+Delta_u;r=r0+Delta_r;I=I0+Delta_i

5、+I_Dot*Delta_t;%10、計(jì)算觀測(cè)歷元t的升交點(diǎn)經(jīng)度OmegakOmegak=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)：nX=%.10

6、fY=%.10fZ=%.10fn,x,y,z);%老師給的結(jié)果：衛(wèi)星位置：X=7073881.4181256806Y=23901970.8378255780Z=-32955601.5025106560X=7073881.4181256806;Y=23901970.837825578;Z=-32955601.5025106560;%修正后的，非GEOfprintf(與老師的結(jié)果的偏差為：nD_X=%.10fD_Y=%.10fD_Z=%.10fn,x-X,y-Y,z-Z);3、程序運(yùn)行結(jié)果(修正后)衛(wèi)星在地固坐標(biāo)系中的坐標(biāo)：X=7073887.5144389411Y=23901969.049612

7、1940Z=-32955601.4908931700與老師的結(jié)果的偏差為：D_X=6.0963132605D_Y=-1.7882133834D_Z=0.0116174854二、已知4顆衛(wèi)星坐標(biāo)及測(cè)得的偽距(已改正)求接收機(jī)位置1、原理及算法不考慮電離層延遲和對(duì)流層延遲的因素時(shí)，由4顆衛(wèi)星在地心坐標(biāo)系中的坐標(biāo)及對(duì)應(yīng)的偽距即可計(jì)算出接收機(jī)的位置，按如下四元二次方程組求解四個(gè)未知數(shù)：R=R+c-At：i0pi=一xX+（y一yX+（z一z）2+cAti=1,2,3,4iii0其中，R為已改正的偽距，R為衛(wèi)星與接收機(jī)之間實(shí)際的距離，(x,y,z)分別為4iiii顆衛(wèi)星的坐標(biāo)，(x,y,z)為接收機(jī)在地

8、心坐標(biāo)系中的坐標(biāo)，At為接收機(jī)時(shí)鐘與衛(wèi)星時(shí)鐘的0偏差值。采用Newton迭代法求解此四元二次非線性方程組，其算法為:將上述方程組寫成如下形式四元方程組：f(x,y,z,A)=0f(x,y,z,A)=0f3(x,y,z,At0)=0f(x,y,z,At)=040記向量X=(x,y,z,At)t，F(xiàn)=(f,f,f,fX，則上式化為：F(X)=001234若給出此方程組的一個(gè)初值X(k)=(x(k),y(k),z(k),At(k)，將函數(shù)F(X)的分量f(X)在0iX(k)處Taylor展開，并取線性部分，則可表示為：F(X人FQ)+人-X(k)再由F(X)=0，可認(rèn)為:1)FfQ（k）Xx-X（k

9、）=-FQ（k）其中Jacobi矩陣為：藥比af2比afT比afj比7ayaf2ayafTayafjayafTaxaf2axafTaxafjax=(X-MaAMaA-MaAMaA則(1)方程組的解為：X(k+1)=X(k)Fk=0,1,2,2)利用(2)式進(jìn)行迭代運(yùn)算即可解出滿足一定精度要求的近似解。若考慮地球自轉(zhuǎn)影響，需要確定地球的自轉(zhuǎn)角速度O以及發(fā)射信號(hào)瞬時(shí)到接收信號(hào)瞬e時(shí)經(jīng)歷的時(shí)間At，從而計(jì)算出衛(wèi)星信號(hào)傳輸時(shí)間內(nèi)地球自轉(zhuǎn)過的角度Ap=QAt，然后利e用旋轉(zhuǎn)矩陣I將上述計(jì)算得到的坐標(biāo)值(x,y,z)做一旋轉(zhuǎn)變換，得出考慮地球自轉(zhuǎn)影響時(shí)接收機(jī)位置(x,y,z)，其旋轉(zhuǎn)操作如下:rrrxry

10、rr廠cosApsinApsinApcosAp01丿其中衛(wèi)星信號(hào)傳輸總時(shí)間At可由最大偽距除以光速得到：At=max(R)/c，(c為光速)。i2、Matlab源程序functionReceiver_Position%本程序求解已知4顆衛(wèi)星坐標(biāo)及測(cè)得的偽距時(shí)接收機(jī)的位置坐標(biāo)XYZ,需要求解四元二次非線性方程組%利用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=

11、1;while(norm(x1-x0)=tol)&nv108)%牛頓迭代計(jì)算x0=x1;x1=x0-inv(df(x0)*fc(x0);%迭代公式n=n+1;endT=toc;fprintf(接收機(jī)坐標(biāo)為：X=%.10fY=%.10fZ=%.10fn,x1(1),x1(2),x1(3);%老師的計(jì)算結(jié)果：x=1725670.7674292959y=-2116958.3717429629z=3129817.7967605507x=1725670.7674292959;y=-2116958.3717429629;z=3129817.7967605507;fprintf(與老師的結(jié)果的偏差為：D_X

12、=%.10fD_Y=%.10fD_Z=%.10fn,x1(1)-x,x1(2)-y,x1(3)-z);fprintf(迭代次數(shù)為：n=%in,n);fprintf(計(jì)算時(shí)間為：T=%fsn,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)瞬時(shí)，地球偏轉(zhuǎn)的角度I=cos(D_phi),si

13、n(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.493646610y=-2116969.3298358698z=3129817.4224583404x=1725657.4936462610;y=-2116969.3298358698;z=3129817.4224583404;fprintf(考慮地球自轉(zhuǎn)因素時(shí)：n接收機(jī)坐標(biāo)為：X=%.10fY=%.10fZ=%.10fn,xr(l),xr(2),xr(3);fprintf(與老師的結(jié)果的偏差為：D_X=%

14、.10fD_Y=%.10fD_Z=%.10fn,xr(1)-x,xr(2)-y,xr(3)-z);end%原方程組functionY=fc(X)c=2.99792458e8;%光速%4顆衛(wèi)星在WGS-84坐標(biāo)系中的的X坐標(biāo)，Y坐標(biāo)，Z坐標(biāo)以及接收機(jī)測(cè)得的對(duì)應(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.428634750

15、e+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).A2+(y-X)42+(z-X(3).a2-(R-c*X(4).a2;%原方程組(矩陣表示)end%Jacobi矩陣functionY=df(X)c=2.99792458e8;%光速%4顆衛(wèi)星在WGS-84坐標(biāo)系中的的X坐標(biāo)，Y坐標(biāo)，Z坐標(biāo)以及接收機(jī)測(cè)得的對(duì)應(yīng)改正后的偽距x=1.4832308660e+007,-

16、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+006;R=2.4310764064e+007,2.2914600784e+007,2.0628809405e+007,2.3422377972e+007;%Y=-2*(x-X(1)-2*(y-X(2)

17、-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矩陣EndD_Z=-0.0000000033D_Z=0.37430220703、程序運(yùn)行結(jié)果接收機(jī)坐標(biāo)為：X