calsatposvel.m

差分载波定位

%给定观测时刻卫星位置计算
function [SatPos,SatVel,deltat]=calSatPosVel(t,orbitPrameters)
%卫星位置，速度计算
% orbitPra.prn      = navData(1,col);
% orbitPra.af2      = navData(2,col);
% % % % % % % nav(1,:)  = svprn;
% % % % % % % nav(2,:)  = af2;
% % % % % % % nav(3,:)  = M0;
% % % % % % % nav(4,:)  = roota;
% % % % % % % nav(5,:)  = deltan;
% % % % % % % nav(6,:)  = ecc;
% % % % % % % nav(7,:)  = omega;
% % % % % % % nav(8,:)  = cuc;
% % % % % % % nav(9,:)  = cus;
% % % % % % % nav(10,:) = crc;
% % % % % % % nav(11,:) = crs;
% % % % % % % nav(12,:) = i0;
% % % % % % % nav(13,:) = idot;
% % % % % % % nav(14,:) = cic;
% % % % % % % nav(15,:) = cis;
% % % % % % % nav(16,:) = Omega0;
% % % % % % % nav(17,:) = Omegadot;
% % % % % % % nav(18,:) = toe;
% % % % % % % nav(19,:) = af0;
% % % % % % % nav(20,:) = af1;
% % % % % % % nav(21,:) = tgd;
M0       = orbitPrameters(3);
SA       = orbitPrameters(4); %轨道长半径平根
Dn       = orbitPrameters(5); %平均运动修正量
e        = orbitPrameters(6);
w        = orbitPrameters(7);
Cuc      = orbitPrameters(8);
Cus      = orbitPrameters(9);
Crc      = orbitPrameters(10);
Crs      = orbitPrameters(11);
i0       = orbitPrameters(12);
idot     = orbitPrameters(13);
Cic      = orbitPrameters(14);
Cis      = orbitPrameters(15);
W0       = orbitPrameters(16);
W        = orbitPrameters(17);
toe      = orbitPrameters(18);
a0 = orbitPrameters(19);
a1 = orbitPrameters(20);
a2 = orbitPrameters(2);
tgd = orbitPrameters(21);

we = 7.2921151467D-05; %地球转动角速度

u = 3986005D+08;       %地球半径
n0 = sqrt(u)/SA^3;     %平均角速度
n = n0 + Dn;           %经摄动改正后的平均角速度

deltat = a0 + a1*(t - toe) + a2*(t - toe)^2 - tgd; %t:     要计算卫星钟差的时刻，即卫星发射信号的时刻 
                                                   %tgd:   电离层延迟改正数
                                                   %deltat:卫星钟差
t = t - deltat; %%GPS卫星发射信号的GPS时间的修正
tk = t - toe;   %规划观测时间
if(tk > 302400) tk = tk - 604800;end
if(tk < -302400) tk = tk + 604800;end
Mk=M0+n*tk; %平近点角
Mkdot = n;

%迭代求偏近点角
Ek=Mk+e*sin(Mk);Ek=Mk+e*sin(Mk);Ek=Mk+e*sin(Mk);Ek=Mk+e*sin(Mk);Ek=Mk+e*sin(Mk);
% while 1
%     Ek_former=Ek;
%     Ek=Mk+e*sin(Mk);
%     if abs(Ek_former-Ek)<0.0000001
%         break;
%     end
% end
%Ek=Mk+e*sin(Mk);Ek=Mk+e*sin(Ek); Ek=Mk+e*sin(Ek);Ek=Mk+e*sin(Ek);Ek=Mk+e*sin(Ek);
deltaTron = -4.442807633e-10*e*SA*sin(Ek); % 相对论效应
tk = tk - deltaTron;
Ekdot = Mkdot/(1.0 - e*cos(Ek));
tak=2*atan(sqrt((1+e)/(1-e))*tan(Ek/2)); %真近点角
takdot = sin(Ek)*Ekdot*(1.0+e*cos(tak))/(sin(tak)*(1.0-e*cos(Ek)));
Ok=tak + w;                    %纬度值，升交距角
Du=Cuc*cos(2*Ok)+Cus*sin(2*Ok);%纬度校正值 
Dr=Crc*cos(2*Ok)+Crs*sin(2*Ok);%半径校正值
Di=Cic*cos(2*Ok)+Cis*sin(2*Ok);%倾角校正值

%%%%%%%%校正后的纬度,半径,倾角,升交点经度
uk=Ok+Du; 
rk=(SA^2)*(1-e*cos(Ek))+Dr; 
ik=i0+Di+idot*tk;
ukdot = takdot +2.0*(Cus*cos(2.0*uk)-Cuc*sin(2.0*uk))*takdot;
rkdot = (SA^2)*e*sin(Ek)*n/(1.0-e*cos(Ek)) + 2.0*(Crs*cos(2.0*uk)-Crc*sin(2.0*uk))*takdot;
ikdot = idot + (Cis*cos(2.0*uk)-Cic*sin(2.0*uk))*2.0*takdot;

%%%%%%%%%%%%在轨道面中的x y值
xk=rk*cos(uk); 
yk=rk*sin(uk); 
xkdot = rkdot*cos(uk) - yk*ukdot;
ykdot = rkdot*sin(uk) + xk*ukdot;

%《GPS技术与应用》P90：(3-58)
Wk=W0+(W-we)*tk-we*toe;
Wk_dot = (W - we);

%%%%%%%%%%%%%%%%%ECEFxyz坐标：卫星位置计算
Xk=xk*cos(Wk)-yk*cos(ik)*sin(Wk); 
Yk=xk*sin(Wk)+yk*cos(ik)*cos(Wk); 
Zk=yk*sin(ik);
SatPos.X = Xk;
SatPos.Y = Yk;
SatPos.Z = Zk;

%**********************卫星速度计算
xkdot = (xkdot-yk*cos(ik)*Wk_dot)*cos(Wk)-(xk*Wk_dot+ykdot*cos(ik)-yk*sin(ik)*ikdot)*sin(Wk);
ykdot = (xkdot-yk*cos(ik)*Wk_dot)*sin(Wk)+(xk*Wk_dot+ykdot*cos(ik)-yk*sin(ik)*ikdot)*cos(Wk);
zkdot = ykdot*sin(ik) + yk*cos(ik)*ikdot;

SatVel.X = xkdot;
SatVel.Y = ykdot;
SatVel.Z = zkdot;