1、GPS Matlab 编程报告一、通过星历计算卫星坐标（修正）1、 算法流程（1） 计算 GPS 卫星运行的平均速度 n0203,sa（2） 计算归化时间 t oet（3） 计算计算观测历元 的平近点角tM0nt（4） 计算偏近点角 E利用不动点迭代法求解此方程： 得出siEeE不动点迭代法的迭代公式为： 10,12kkx（5） 计算卫星的地心矢径 0r0cosaeE（6） 计算真近点角 f 12*arctn(ta)2ef（7） 计算升交点角距 00f（8） 计算摄动改正项： ,uri00sn2cosiuurriisicC（9） 计算经过摄动改正的升交点角距 ，卫星矢径 ，和轨道面倾角 i00
2、uriit（10） 计算观测历元 的升交点经度tk0kieieot（11） 计算卫星在轨道平面坐标系中的位置 0,xyz0csin rz（12） 计算卫星在地固坐标系下的坐标 ,xy00coscsin()inokkzkxx yyRizizi 2、 Matlab 源程序%本程序用于通过星历计算卫星坐标（修正）%部分星历数据未用到clear;%-t 时刻，卫星星历数据-t=1.728128741751984e+005; %当前时间（接收到卫星信号时间）Omega0= -3.1122016819; %按参考时间计算的升交点经度I0=0.3*pi; %非 GEO 卫星 %I0= 0.087996906
3、8; %GEO 卫星SqrtA= 6493.27499625; %长半轴平方根Ecc= 0.01995786; %偏心率Small_Omega= -1.7409228484; %近地点角距Mu0= 0.0005884201; %参考时间的平近点角Delta_n= -2.4123e-009; %卫星平均运动速度与计算值之差I_Dot= -3.4333e-010; %轨道倾角变化率Omega_dot= 3.47676e-009; %升交点经度变化率 C_uc= 4.692e-006; %纬度幅角的余弦调和改正项的振幅C_us= -1.10036e-005; %纬度幅角的正弦调和改正项的振幅C_ic
4、= 9.01e-008; %轨道倾角的余弦调和改正项的振幅C_is= 2.01e-008; %轨道倾角的正弦调和改正项的振幅C_rc= 325.73368; %轨道半径的余弦调和改正项的振幅C_rs= 146.91074; %轨道半径的正弦调和改正项的振幅Toe= 172800; %星历参考时间IODC= 10; %钟差数据龄期URAI= 0; %用户距离精度标志IODE= 10; %星历数据龄期Toc = 172800; %本时段钟差参数参考时间a0 = -2.72893e-005; %卫星钟差改正 0 阶多项式系数 a1 = -6.7531e-014; %卫星钟差改正 1 阶多项式系数a2
5、 = 5.787e-018; %卫星钟差改正 2 阶多项式系数%-定义常量-c=2.99792458e8; %光速omegae=7.2921151467e-5; %地球自转角速度 mu=3.986004418e14; %地球引力常数 GM%-1、计算 GPS 卫星运行的平均速度 n-a=SqrtA*SqrtA;n=sqrt(mu/(a3)+Delta_n;%-2、计算归化时间 Delta_t-Delta_t=t-Toe;%-3、计算观测历元 t 的平近点角 M-M=Mu0+n*Delta_t;%-4、计算偏近点角 E-eps=1e-20;E=M;tol=1;while (toleps) %不动
6、点迭代法E0=E;E=M+(Ecc)*sin(E0);tol=abs(E-E0);end%-5、计算卫星的地心矢径 r0-r0=a*(1-Ecc*cos(E);%-6、计算真近点角 f-%f=2*atan(sqrt(1+Ecc)/(1-Ecc)*tan(E/2);f=atan(sqrt(1-Ecc2)*sin(E)/(cos(E)-Ecc);%-7、计算升交点角距 Phi0-Phi0=Small_Omega+f;%-8、计算摄动改正项：Delta_u,Delta_r,Delta_i-Delta_u=C_us*sin(2*Phi0)+C_uc*cos(2*Phi0);Delta_r=C_rs*s
7、in(2*Phi0)+C_rc*cos(2*Phi0);Delta_i=C_is*sin(2*Phi0)+C_ic*cos(2*Phi0);%-9、计算经过摄动改正的升交点角距 Phi，卫星矢径 r，和轨道面倾角 I-Phi=Phi0+Delta_u;r=r0+Delta_r;I=I0+Delta_i+I_Dot*Delta_t;%-10、计算观测历元 t 的升交点经度 Omegak-Omegak=Omega0+(Omega_dot-omegae)*Delta_t-omegae*Toe;%-11、计算卫星在轨道平面坐标系中的位置 -x0=r*cos(Phi);y0=r*sin(Phi);z0=
8、0;%-12、计算卫星在地固坐标系下的坐标 -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);%-输出卫星坐标-fprintf (（修正后）卫星在地固坐标系中的坐标：n X=%.10f Y=%.10f Z=%.10fn,x,y,z);%老师给的结果：卫星位置：X=7073881.4181256806 Y=23901970.8378255780 Z=-32955601.5025106560X=7073881.4181256806;Y=239019
9、70.837825578;Z=-32955601.5025106560;%修正后的，非 GEOfprintf (与老师的结果的偏差为：n D_X=%.10f D_Y=%.10f D_Z=%.10fn,x-X,y-Y,z-Z);3、 程序运行结果（修正后）卫星在地固坐标系中的坐标：X=7073887.5144389411 Y=23901969.0496121940 Z=-32955601.4908931700与老师的结果的偏差为：D_X=6.0963132605 D_Y=-1.7882133834 D_Z=0.0116174854二、已知 4 颗卫星坐标及测得的伪距（已改正）求接收机位置1、 原
10、理及算法不考虑电离层延迟和对流层延迟的因素时，由 4 颗卫星在地心坐标系中的坐标及对应的伪距即可计算出接收机的位置，按如下四元二次方程组求解四个未知数： 2220 0 1,234i iiiRctxyzcti其中， 为已改正的伪距， 为卫星与接收机之间实际的距离， 分别为 4i R()iixyz颗卫星的坐标， 为接收机在地心坐标系中的坐标， 为接收机时钟与卫星时钟的,xyz 0t偏差值。采用 Newton 迭代法求解此四元二次非线性方程组，其算法为：将上述方程组写成如下形式四元方程组： 102304,fxyztftxyz记向量 ， ，则上式化为：0,TxyztX123,TffFFX0若给出此方程
11、组的一个初值 ，将函数 的分量 在0,TkkkxyztXif处 Taylor 展开，并取线性部分，则可表示为：k kkkFFX再由 ，可认为：F0（1）kkk其中 Jacobi 矩阵为： 1102233044ffxyztfftffxyztfftFX则（1）方程组的解为：（2）11 =0,12kkkkXFX利用（2）式进行迭代运算即可解出满足一定精度要求的近似解。若考虑地球自转影响，需要确定地球的自转角速度 以及发射信号瞬时到接收信号瞬e时经历的时间 ，从而计算出卫星信号传输时间内地球自转过的角度 ，然后t et利用旋转矩阵 将上述计算得到的坐标值 做一旋转变换，得出考虑地球自转影响I,xyz时
12、接收机位置 ，其旋转操作如下：,rrxyzcosin0i01rrxxyyz zI其中卫星信号传输总时间 可由最大伪距除以光速得到： ， （ 为光速） 。tmax/itRc2、 Matlab 源程序function Receiver_Position%本程序求解已知 4 颗卫星坐标及测得的伪距时接收机的位置坐标X Y Z，需要求解四元二次非线性方程组%利用 Newton 法求解此非线性方程组clear;tic;c=2.99792458e8; %光速omegae=7.2921151467e-5;%地球自转角速度x0=0;0;0;0; %取初始值tol=1.0e-6; %计算精度x1=x0-inv(
13、df(x0)*fc(x0); %计算第一个迭代值n=1;while(norm(x1-x0)=tol)x1=x0-inv(df(x0)*fc(x0); %迭代公式n=n+1;endT=toc;fprintf (接收机坐标为：X=%.10f Y=%.10f Z=%.10fn,x1(1),x1(2),x1(3);%老师的计算结果： x=1725670.7674292959 y=-2116958.3717429629 z=3129817.7967605507x=1725670.7674292959;y=-2116958.3717429629;z=3129817.7967605507;fprintf (
14、与老师的结果的偏差为：D_X=%.10f D_Y=%.10f D_Z=%.10fn,x1(1)-x,x1(2)-y,x1(3)-z);fprintf (迭代次数为：n=%in,n);fprintf (计算时间为：T=%f sn,T);fprintf (计算精度为：tol=%fnn,tol);%考虑地球自转因素R=2.4310764064e+007,2.2914600784e+007,2.0628809405e+007,2.3422377972e+007; %伪距D_t=max(R)/c; %取最大的伪距除以光速得到卫星信号传输时间D_phi=omegae*D_t; %发射信号瞬时到接收信号瞬时
15、，地球偏转的角度I=cos(D_ phi), sin(D_ phi), 0; -sin(D_ phi), cos(D_ phi) , 0; 0, 0, 1; %旋转矩阵xr=I*x1(1);x1(2);x1(3); %旋转后坐标%老师的计算结果： x=1725657.493646610 y=-2116969.3298358698 z=3129817.4224583404x=1725657.4936462610;y=-2116969.3298358698;z=3129817.4224583404;fprintf (考虑地球自转因素时：n 接收机坐标为：X=%.10f Y=%.10f Z=%.10
16、fn,xr(1),xr(2),xr(3);fprintf (与老师的结果的偏差为：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 颗卫星在 WGS-84 坐标系中的的 X 坐标，Y 坐标，Z 坐标以及接收机测得的对应改正后的伪距x=1.4832308660e+007,-1.5799854050e+007,1.984818910e+006,-1.2480273190e+007;y=2.0466715890e+007,-1.33011291
17、70e+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 Y=df(X)c=2.99792458e8; %光速%4
18、颗卫星在 WGS-84 坐标系中的的 X 坐标，Y 坐标，Z 坐标以及接收机测得的对应改正后的伪距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+006;R=2.4310764064e+007,2.29146
19、00784e+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
20、),-2*(z(4)-X(3),2*c*(R(4)-c*X(4);%Jacobi 矩阵End3、 程序运行结果接收机坐标为：X=1725670.7674292948 Y=-2116958.3717429619 Z=3129817.7967605474与老师的结果的偏差为：D_X=-0.0000000012 D_Y=0.0000000009 D_Z=-0.0000000033迭代次数为：n=5计算时间为：T=0.001102 s计算精度为：tol=0.000001考虑地球自转因素时：接收机坐标为：X=1725658.2491456384 Y=-2116968.5761503954 Z=3129817.7967605474与老师的结果的偏差为：D_X=0.7554993774 D_Y=0.7536854744 D_Z=0.3743022070
