了解了组合导航紧耦合的框架后,下一步就是准备GNSS的输入数据了。
这里以单点定位的代码为例,介绍每个参数的计算方法:
代码链接在这里:
kakusang2020/iXR_GNSS-IMU_TightlyCouplingProgramgithub.com
代码框图如下所示:
读取RINEX3.02格式的星历文件,之后计算卫星速度,位置等信息。这里以单点定位程序为基础,在红色标注的地方存储紧耦合需要的数据。
1.GNSS时间
选择GPS秒为时间基准。
2.PRN号
卫星编号,GPS一般从1遍到32号,QZSS从33到40.
3.伪距
精确的伪距需要电离层,对流层,卫星钟差和相对论效应改正。改正过后的伪距保留着接收机钟差,紧耦合中钟差和钟差率的状态量不为0。
L_noclk=[L_noclk;Range-Trop+c*sat_clk+c*rela];%计算并存储伪距
4.卫星位置
这里的位置是接受时刻卫星在ECEF下的位置。
参考《GPS原理与接收机设计》P62:
求出t时刻与参考时间toe之间的差异;
tk=tkr-t1; %The time difference between the time when the satellite transmits the signal and the reference time
计算信号发射时刻平近点角
Mk=tempNav.M0+n*tk; % Mean anomaly (rad/s)
计算发射时刻偏近角点,用迭代法从开普勒方程中解出
while dEk>1e-9
Ek=Mk+e*sin(Ek0);
dEk=abs(Ek-Ek0);
Ek0=Ek;
end
计算信号发射时刻的真近角点
vk=2*atan(tan(Ek/2)*sqrt((1+e)/(1-e))); % True anom (rad)
if(vk<0)
vk=vk+2*pi;
end
升交点角距
PHI_k=vk+tempNav.omega;
计算摄动校正项
duk=tempNav.cus*sin(2*PHI_k)+tempNav.cuc*cos(2*PHI_k); % Argument of Lat correction
drk=tempNav.crs*sin(2*PHI_k)+tempNav.crc*cos(2*PHI_k); % Radius correction
dik=tempNav.cis*sin(2*PHI_k)+tempNav.cic*cos(2*PHI_k); % Inclination correction
计算升交点角距,卫星失径长度和轨道倾角
uk=PHI_k+duk; % Corr. arg of lat
r=A*(1-e*cos(Ek))+drk; % Corrected radius
ik=tempNav.i0+dik+tempNav.idot*tk; % Corrected inclination
信号发射时刻卫星在轨道平面位置
x=r*cos(uk);
y=r*sin(uk);
升交点赤经
Omega=tempNav.Omega0+(tempNav.Omegadot-omgedote)*tk-omgedote*tempNav.toe
卫星位置
% ECEF coordinates
Xs=x*cos(Omega)-y*cos(ik)*sin(Omega);
Ys=x*sin(Omega)+y*cos(ik)*cos(Omega);
Zs=y*sin(ik);
5.伪距率
rate = -1.0 * ObsData(i).svObs(j).measurements(D1C) * c / F1;
LV_ = [LV_; rate + ( AV_(end,1) .* (Xsvel-ApproCoorV_(1)) + AV_(end,2) .* (Ysvel-ApproCoorV_(2)) + AV_(end,3) .* (Zsvel-ApproCoorV_(3))) - c * delta_tsv_L1pie];%实际的伪距率(卫星速度减接收机速度)减去卫星时钟频漂
6.卫星钟差率
导航文件的af1加af2乘以(观测时刻-星历参考时刻)
rate_clock = tempNav.af1 + tempNav.af2 * t;
7.卫星速度
偏近点角对时间求导
ek=mk/(1-tempNav.ecc*cos(Ek));
升交点角距求导
Wk=sqrt(1-tempNav.ecc^2)*ek/(1-tempNav.ecc*cos(Ek));
摄动校正项求导
Uuk=2*Wk*(tempNav.cus*cos(2*PHI_k)-tempNav.cuc*sin(2*PHI_k)); %Perturbation correction term
Rrk=2*Wk*(tempNav.crs*cos(2*PHI_k)-tempNav.crc*sin(2*PHI_k));
Iik=2*Wk*(tempNav.cis*cos(2*PHI_k)-tempNav.cic*sin(2*PHI_k));
对升交点角距,卫星失径长度,轨道倾角升交点赤经求导
Uk=Wk+Uuk; %After correction
Rk=(tempNav.roota^2)*tempNav.ecc*ek*sin(Ek)+Rrk;
Ik=tempNav.idot+Iik;
WK=tempNav.Omegadot-7.2921151467e-5;
信号发射时刻卫星在轨道平面位置求导
Xxk=Rk*cos(uk)-r*Uk*sin(uk);
Yyk=Rk*sin(uk)+r*Uk*cos(uk);
同样是接受时刻卫星在ECEF下的速度
Xsvel=-Ys*WK-(Yyk*cos(ik)-Zs*Ik)*sin(Omega)+Xxk*cos(Omega);
Ysvel=Xs*WK+(Yyk*cos(ik)-Zs*Ik)*cos(Omega)+Xxk*sin(Omega);
Zsvel=Yyk*sin(ik)+y*Ik*cos(ik);
8.卫星高度角
接收机水平方向为0度,天顶为90度。
Sat_xyz=([Xs ,Ys ,Zs]-ApproCoor)*Rota;
ele=asind(Sat_xyz(3)/sqrt(sum(Sat_xyz.^2)));
9.SNR信息
每个卫星的信噪比,作为权衡信号质量的值。对应星历文件的S1C。
ObsData(i).svObs(j).measurements(S1C)
有了这些信息后就可以作为GNSS量测信息传入紧耦合程序中啦。