当前位置：首页 > news >正文

多模卫星导航定位与应用-原理与实践（RTKLib）3

news 2025/10/7 5:12:12

单点精度为m级，使用星历文件，红色的为未知数

l为验前残差

组法方程，最小二乘法求解

rtklib原作者不更新了，新的库是rtklib explorer

https://github.com/rtklibexplorer/RTKLIB/releases

现在的版本是2.5.0，可直接下载exe，也可通过VS2019+QT5编译生成

打开rtkpost，10种定位处理模式，Single伪距单点，第2-第6差分，最后3个是PPP

地心地固坐标系（Earth-Centered, Earth-Fixed，简称ECEF）简称地心坐标系，空间直角坐标

坐标原点与地球质心重合;
z轴指向协议地极；
x轴指向赤道与本初子午线(格林尼治子午线)的交点;
y轴在赤道平面上与xxx轴和zzz轴构成右手笛卡儿坐标系。

rtkcmn.c

enu站心坐标系

转换到enu的协方差

协方差，又称共变异数，被用来描述两个随机变量之间线性相关程度，常用的符号有cov(X, Y)，σ(X, Y)等。

协方差 cov(X, Y) 定义为两个随机变量X和Y偏离其期望值的乘积的期望，即cov(X,Y) = E[(X - E[X])(Y - E[Y])] [1]。其中：E[X] 和 E[Y] 分别是随机变量 X 和 Y 的期望值， cov 是协方差的英文 “covariance” 的缩写。当协方差为正值时，表明随机变量X和Y倾向于同时偏离其平均值，呈正相关关系；反之，若协方差为负值，则表明一个变量高于平均值时，另一个倾向于低于平均值，呈负相关关系。如果协方差为零，这意味着两个变量之间没有线性关系。

绕X、Y、Z轴旋转矩阵

地心惯性坐标系（ECI）
在空间保持静止或匀速直线运动(无加速度)的坐标系称为惯性坐标系。所有惯性器件在测量轴方向产生的都是惯性系下的测量结果。此外,还需要在惯性系下完成卫星绕地球的位置和速度的估计。对于近地卫星,通常采用地心惯性(ECI)坐标系。

地球质心为坐标系原点;
z轴沿着地球自转轴指向协议地极;
x轴位于赤道平面内,并指向春分点;
y轴符合右手笛卡儿坐标系。

跳秒，UTC和GPS时

bdt北斗时，gpstGPS时

转年积日

卫星编号satno

#define SYS_NONE 0x00 /* navigation system: none */
#define SYS_GPS 0x01 /* navigation system: GPS */
#define SYS_SBS 0x02 /* navigation system: SBAS */
#define SYS_GLO 0x04 /* navigation system: GLONASS */
#define SYS_GAL 0x08 /* navigation system: Galileo */
#define SYS_QZS 0x10 /* navigation system: QZSS */
#define SYS_CMP 0x20 /* navigation system: BeiDou */
#define SYS_IRN 0x40 /* navigation system: IRNS */
#define SYS_LEO 0x80 /* navigation system: LEO */
#define SYS_ALL 0xFF /* navigation system: all */

#define MINPRNGPS 1 /* min satellite PRN number of GPS */
#define MAXPRNGPS 32 /* max satellite PRN number of GPS */
#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */
#define NSYSGPS 1

#ifdef ENAGLO
#define MINPRNGLO 1 /* min satellite slot number of GLONASS */
#define MAXPRNGLO 27 /* max satellite slot number of GLONASS */
#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */
#define NSYSGLO 1
#else
#define MINPRNGLO 0
#define MAXPRNGLO 0
#define NSATGLO 0
#define NSYSGLO 0
#endif
#ifdef ENAGAL
#define MINPRNGAL 1 /* min satellite PRN number of Galileo */
#define MAXPRNGAL 36 /* max satellite PRN number of Galileo */
#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */
#define NSYSGAL 1
#else
#define MINPRNGAL 0
#define MAXPRNGAL 0
#define NSATGAL 0
#define NSYSGAL 0
#endif
#ifdef ENAQZS
#define MINPRNQZS 193 /* min satellite PRN number of QZSS */
#define MAXPRNQZS 202 /* max satellite PRN number of QZSS */
#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS L1S */
#define MAXPRNQZS_S 191 /* max satellite PRN number of QZSS L1S */
#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */
#define NSYSQZS 1
#else
#define MINPRNQZS 0
#define MAXPRNQZS 0
#define MINPRNQZS_S 0
#define MAXPRNQZS_S 0
#define NSATQZS 0
#define NSYSQZS 0
#endif
#ifdef ENACMP
#define MINPRNCMP 1 /* min satellite sat number of BeiDou */
#define MAXPRNCMP 46 /* max satellite sat number of BeiDou */
#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */

SBAS 星基增强系统

#define MINPRNSBS 120 /* min satellite PRN number of SBAS */
#define MAXPRNSBS 158 /* max satellite PRN number of SBAS */
#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */

C01转成satno

satno转成C01

单点定位pntpos.c

/* single-point positioning ----------------------------------------------------
* compute receiver position, velocity, clock bias by single-point positioning
* with pseudorange and doppler observables
* args : obsd_t *obs I observation data
* int n I number of observation data
* nav_t *nav I navigation data
* prcopt_t *opt I processing options
* sol_t *sol IO solution
* double *azel IO azimuth/elevation angle (rad) (NULL: no output)
* ssat_t *ssat IO satellite status (NULL: no output)
* char *msg O error message for error exit
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg)
{
prcopt_t opt_=*opt;
double *rs,*dts,*var,*azel_,*resp;
int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];

char tstr[40];
trace(3,"pntpos : tobs=%s n=%d\n",time2str(obs[0].time,tstr,3),n);

sol->stat=SOLQ_NONE;

if (n<=0) {
strcpy(msg,"no observation data");
return 0;
}
sol->time=obs[0].time;
msg[0]='\0';
sol->eventime = obs[0].eventime;

rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);

if (ssat) {
for (i=0;i<MAXSAT;i++) {
ssat[i].snr_rover[0]=0;
ssat[i].snr_base[0]=0;
}
for (i=0;i<n;i++)
ssat[obs[i].sat-1].snr_rover[0]=obs[i].SNR[0];
}

if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
opt_.ionoopt=IONOOPT_BRDC;
opt_.tropopt=TROPOPT_SAAS;
}
/* satellite positions, velocities and clocks */
satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);

/* estimate receiver position and time with pseudorange */
stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,ssat,sol,azel_,vsat,resp,msg);

/* RAIM FDE */
if (!stat&&n>=6&&opt->posopt[4]) {
stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,ssat,sol,azel_,vsat,resp,msg);
}
/* estimate receiver velocity with Doppler */
if (stat) {
estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
}
if (azel) {
for (i=0;i<n*2;i++) azel[i]=azel_[i];
}
if (ssat) {
for (i=0;i<MAXSAT;i++) {
ssat[i].vs=0;
ssat[i].azel[0]=ssat[i].azel[1]=0.0;
ssat[i].resp[0]=ssat[i].resc[0]=0.0;
}
for (i=0;i<n;i++) {
ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
if (!vsat[i]) continue;
ssat[obs[i].sat-1].vs=1;
ssat[obs[i].sat-1].resp[0]=resp[i];
}
}
free(rs); free(dts); free(var); free(azel_); free(resp);
return stat;
}

按照观测数据的顺序，首先将将当前观测卫星的 rs、dts、var和svh数组的元素置 0。
通过判断某一频率下信号的伪距是否为 0，来得到此时所用的频率个数。注意，频率个数不能大于 NFREQ（默认为 3）。
用数据接收时间减去伪距信号传播时间，得到卫星信号的发射时间。
调用 ephclk函数，由广播星历计算出当前观测卫星的钟差。注意，此时的钟差是没有考虑相对论效应和 TGD（Tgd：the time group delay卫星内部 L1、L2信号从产生到发射的时延之差。当使用单频接收机时，用Tgd改正所观测的结果，以减小电离层效应影响提高定位精度；当采用双频接收机时，就不必要采用这个时延差改正。该值在定位计算中，最终应用在卫星钟差计算中。）的。
用 3中的信号发射时间减去 4中的钟偏，得到 GPS时间下的卫星信号发射时间。
调用 satpos函数，计算信号发射时刻卫星的 P(ecef,m)、V(ecef,m/s)、C((s|s/s))。注意，这里计算出的钟差是考虑了相对论效应的了，只是还没有考虑 TGD。
如果由 6中计算出的钟偏为 0，就再次调用 ephclk函数，将其计算出的卫星钟偏作为最终的结果。

extern void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
int ephopt, double *rs, double *dts, double *var, int *svh)
{
gtime_t time[2*MAXOBS]={{0}};
double dt,pr;
int i,j;

char tstr[40];
trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time2str(teph,tstr,3),n,ephopt);

for (i=0;i<n&&i<2*MAXOBS;i++) {
for (j=0;j<6;j++) rs [j+i*6]=0.0;
for (j=0;j<2;j++) dts[j+i*2]=0.0;
var[i]=0.0; svh[i]=0;

/* search any pseudorange */
for (j=0,pr=0.0;j<NFREQ;j++) if ((pr=obs[i].P[j])!=0.0) break;

if (j>=NFREQ) {
trace(2,"no pseudorange %s sat=%2d\n",time2str(obs[i].time,tstr,3),obs[i].sat);
continue;
}
/* transmission time by satellite clock */
time[i]=timeadd(obs[i].time,-pr/CLIGHT);

/* satellite clock bias by broadcast ephemeris */
if (!ephclk(time[i],teph,obs[i].sat,nav,&dt)) {
trace(3,"no broadcast clock %s sat=%2d\n",time2str(time[i],tstr,3),obs[i].sat);
continue;
}
time[i]=timeadd(time[i],-dt);

/* satellite position and clock at transmission time */
if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i,
svh+i)) {
trace(3,"no ephemeris %s sat=%2d\n",time2str(time[i],tstr,3),obs[i].sat);
continue;
}
/* if no precise clock available, use broadcast clock instead */
if (dts[i*2]==0.0) {
if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue;
dts[1+i*2]=0.0;
*var=SQR(STD_BRDCCLK);
}
trace(4,"satposs: %d,time=%.9f dt=%.9f pr=%.3f rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f\n",
obs[i].sat,time[i].sec,dt,pr,rs[i*6],rs[1+i*6],rs[2+i*6],dts[i*2]*1E9,
var[i]);

}
for (i=0;i<n&&i<2*MAXOBS;i++) {
trace(4,"%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
time2str(time[i],tstr,9),obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],
dts[i*2]*1E9,var[i],svh[i]);
}
}

}
/* satellite position and clock ------------------------------------------------
* compute satellite position, velocity and clock
* args : gtime_t time I time (gpst)
* gtime_t teph I time to select ephemeris (gpst)
* int sat I satellite number
* int ephopt I ephemeris option (EPHOPT_???)
* nav_t *nav I navigation data
* double *rs O sat position and velocity (ecef)
* {x,y,z,vx,vy,vz} (m|m/s)
* double *dts O sat clock {bias,drift} (s|s/s)
* double *var O sat position and clock error variance (m^2)
* int *svh O sat health flag (-1:correction not available)
* return : status (1:ok,0:error)
* notes : satellite position is referenced to antenna phase center
* satellite clock does not include code bias correction (tgd or bgd)
*-----------------------------------------------------------------------------*/
extern int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
const nav_t *nav, double *rs, double *dts, double *var,
int *svh)
{
char tstr[40];
trace(4,"satpos : time=%s sat=%2d ephopt=%d\n",time2str(time,tstr,3),sat,ephopt);

*svh=0;

switch (ephopt) {
case EPHOPT_BRDC : return ephpos (time,teph,sat,nav,-1,rs,dts,var,svh);
case EPHOPT_SBAS : return satpos_sbas(time,teph,sat,nav, rs,dts,var,svh);
case EPHOPT_SSRAPC: return satpos_ssr (time,teph,sat,nav, 0,rs,dts,var,svh);
case EPHOPT_SSRCOM: return satpos_ssr (time,teph,sat,nav, 1,rs,dts,var,svh);
case EPHOPT_PREC :
if (!peph2pos(time,sat,nav,1,rs,dts,var)) break; else return 1;
}
*svh=-1;
return 0;
}

BRDC广播星历，PREC精密星历

/* satellite position and clock by broadcast ephemeris -----------------------*/
static int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int iode, double *rs, double *dts, double *var, int *svh)
{
eph_t *eph;
geph_t *geph;
seph_t *seph;
double rst[3],dtst[1],tt=1E-3;
int i,sys;

char tstr[40];
trace(4,"ephpos : time=%s sat=%2d iode=%d\n",time2str(time,tstr,3),sat,iode);

sys=satsys(sat,NULL);

*svh=-1;

if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP||sys==SYS_IRN) {
if (!(eph=seleph(teph,sat,iode,nav))) return 0;
eph2pos(time,eph,rs,dts,var);
time=timeadd(time,tt);
eph2pos(time,eph,rst,dtst,var);
*svh=eph->svh;
}
else if (sys==SYS_GLO) {
if (!(geph=selgeph(teph,sat,iode,nav))) return 0;
geph2pos(time,geph,rs,dts,var);
time=timeadd(time,tt);
geph2pos(time,geph,rst,dtst,var);
*svh=geph->svh;
}
else if (sys==SYS_SBS) {
if (!(seph=selseph(teph,sat,nav))) return 0;
seph2pos(time,seph,rs,dts,var);
time=timeadd(time,tt);
seph2pos(time,seph,rst,dtst,var);
*svh=seph->svh;
}
else return 0;

/* satellite velocity and clock drift by differential approx */
for (i=0;i<3;i++) rs[i+3]=(rst[i]-rs[i])/tt;
dts[1]=(dtst[0]-dts[0])/tt;

return 1;
}

将 sol->rr的前 3项赋值给 x数组。
开始迭代定位计算，首先调用 rescode函数（计算伪距残差），计算在当前接收机位置和钟差值的情况下，定位方程的右端部分 v(nv\*1)、几何矩阵 H(NX*nv)、此时所得的伪距残余的方差 var、所有观测卫星的 azel{方位角、仰角}、定位时有效性 vsat、定位后伪距残差 resp、参与定位的卫星个数 ns和方程个数 nv。
确定方程组中方程的个数要大于未知数的个数。
以伪距残余的标准差的倒数作为权重，对 H和 v分别左乘权重对角阵，得到加权之后的 H和 v。
调用 lsq函数，根据和，得到当前 x的修改量和定位误差协方差矩阵中的权系数阵。
将 5中求得的 x加入到当前 x值中，得到更新之后的 x值。
如果 5中求得的修改量小于截断因子(目前是1e-4)，则将 6中得到的 x值作为最终的定位结果，对 sol的相应参数赋值，之后再调用 valsol函数确认当前解是否符合要求（伪距残余小于某个值和 GDOP小于某个门限值）。否则，进行下一次循环。
如果超过了规定的循环次数，则输出发散信息后，返回 0。

/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, const ssat_t *ssat, sol_t *sol, double *azel,
int *vsat, double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;

trace(3,"estpos : n=%d\n",n);

v=mat(n+NX-3,1); H=mat(NX,n+NX-3); var=mat(n+NX-3,1);

for (i=0;i<3;i++) x[i]=sol->rr[i];

for (i=0;i<MAXITR;i++) {

/* pseudorange residuals (m) */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,ssat,v,H,var,azel,vsat,resp,
&ns);

if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance (lsq uses sqrt of weight */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++) {
x[j]+=dx[j];
}
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* GLO-GPS time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* GAL-GPS time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* BDS-GPS time offset (s) */
sol->dtr[4]=x[7]/CLIGHT; /* IRN-GPS time offset (s) */
#ifdef QZSDT
sol->dtr[5]=x[8]/CLIGHT; /* QZS-GPS time offset (s) */
#endif
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(uint8_t)ns;
sol->age=sol->ratio=0.0;

/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);

free(v); free(H); free(var);
return 0;
}