rtkpos函数在procpos函数下运行
rtkpos函数调用流程图
1、rtkpos函数：计算接收机的位置、速度和钟差
rtkpos函数根据观测数据和导航信息，计算接收机的位置、速度和钟差。 设置基准站位置，记录观测值数量
/* precise positioning ---------------------------------------------------------
* input observation data and navigation message, compute rover position by
* precise positioning
* rtk_t *rtk RTK控制结构体
* const obsd_t *obs 观测数据OBS
* int n 观测数据数量
* const nav_t *nav 导航电文信息
* return : status (0:no solution,1:valid solution)
* notes : before calling function, base station position rtk->sol.rb[] should
* be properly set for relative mode except for moving-baseline
*-----------------------------------------------------------------------------*/
extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)1.1主程序处理过程
初始化定义
prcopt_t *opt=&rtk->opt; //这里定义了一个prcopt_t用来储存传入的rtk_t中的prcopt_t sol_t solb={{0}}; gtime_t time; int i,nu,nr; char msg[128]=""; trace(3,"rtkpos : time=%s n=%d\n",time_str(obs[0].time,3),n); trace(4,"obs=\n"); traceobs(4,obs,n); //设置rtk内基准站坐标，基准站坐标在execses函数内已经计算了，速度设为0.0 //这里将配置结构体opt内基准站的坐标赋值给解算结构体rtk内基准站的坐标 /* set base staion position */ if (opt->refpos<=POSOPT_RINEX&&opt->mode!=PMODE_SINGLE&& opt->mode!=PMODE_MOVEB) { for (i=0;i<6;i++) rtk->rb[i]=i<3?opt->rb[i]:0.0; //opt内基准站坐标赋值给rtk->rb,速度设为0.0 }计算移动站和基站的观测值数量nu和nr
/* count rover/base station observations */ //统计基准站OBS个数nu，流动站OBS个数nr，可用于后面判断是否满足差分条件 for (nu=0;nu <n&&obs[nu ].rcv==1;nu++) ; for (nr=0;nu+nr<n&&obs[nu+nr].rcv==2;nr++) ; time=rtk->sol.time; /* previous epoch */不论定位模式，调用pntpos函数先进行一次接收机单点定位
//利用观测值及星历计算流动站的SPP定位结果，作为kalman滤波的近似坐标。需要注意， //如果由于流动站SPP定位结果坐标误差过大等原因导致的SPP无解，则不进行rtk运算，当前历元无解。 /* rover position by single point positioning */ if (!pntpos(obs,nu,nav,&rtk->opt,&rtk->sol,NULL,rtk->ssat,msg)) { errmsg(rtk,"point pos error (%s)\n",msg); if (!rtk->opt.dynamics) { outsolstat(rtk); return 0; } } if (time.time!=0) rtk->tt=timediff(rtk->sol.time,time);若为单点定位模式，输出，返回
/* single point positioning */ if (opt->mode==PMODE_SINGLE) { //单点定位模式直接输出刚刚SPP算的坐标 outsolstat(rtk); return 1; } //如果不是单点模式，抑制单点解的输出， /* suppress output of single solution */ if (!opt->outsingle) { rtk->sol.stat=SOLQ_NONE; }若为 PPP 模式，调用pppos函数进行精密单点定位，输出，返回
/* precise point positioning */ //精密单点定位 if (opt->mode>=PMODE_PPP_KINEMA) { pppos(rtk,obs,nu,nav); outsolstat(rtk); return 1; }若无基准站观测数据，输出，返回
//检查该历元流动站观测时间和基准站观测时间是否对应，若无基准站观测数据，return /* check number of data of base station and age of differential */ if (nr==0) { errmsg(rtk,"no base station observation data for rtk\n"); outsolstat(rtk); return 1; }若为移动基站模式，调用pntpos函数进行动基站的单点定位，并加以时间同步；否则只计算一下差分时间
//动基线与其他差分定位方式，动基线的基站坐标需要随时间同步变化，所以需要计算出变化速率, //解释了为什么第二步除了单点定位，动基线也不参与基站解算，动基线在这里单独解算 if (opt->mode==PMODE_MOVEB) { /* moving baseline */ //若为移动基线模式 /* estimate position/velocity of base station */ //spp计算基准站位置 if (!pntpos(obs+nu,nr,nav,&rtk->opt,&solb,NULL,NULL,msg)) { errmsg(rtk,"base station position error (%s)\n",msg); return 0; } rtk->sol.age=(float)timediff(rtk->sol.time,solb.time); //计算差分龄期rtk->sol.age if (fabs(rtk->sol.age)>TTOL_MOVEB) { errmsg(rtk,"time sync error for moving-base (age=%.1f)\n",rtk->sol.age); return 0; } for (i=0;i<6;i++) rtk->rb[i]=solb.rr[i]; //把solb.rr赋值给rtk->rb /* time-synchronized position of base station */ //时间同步 for (i=0;i<3;i++) rtk->rb[i]+=rtk->rb[i+3]*rtk->sol.age; //位置+=对应速度*差分龄期 } //不为移动基站模式，只计算一下差分时间 else { rtk->sol.age=(float)timediff(obs[0].time,obs[nu].time); if (fabs(rtk->sol.age)>opt->maxtdiff) { errmsg(rtk,"age of differential error (age=%.1f)\n",rtk->sol.age); outsolstat(rtk); return 1; } }调用relpos函数进行相对基站的接收机定位，输出，返回
·相对定位模式在调用rtkpos之前应该先设置好基站位置，动基线模式除外
//上面的步骤只算了相对定位的差分时间和动基线坐标,这里进行相位定位，并输出最终结果，到这里定位步骤全部完成 //相对定位算法的核心函数 /* relative potitioning */ relpos(rtk,obs,nu,nr,nav); outsolstat(rtk); return 1; }
2. 调用函数
2.1 pntpos函数：实现单点定位
详见RTKLIB源码学习之单点定位pntpos.c http://t.csdnimg.cn/d33QK
2.2 pppos函数：实现精密单点定位
pppos函数实现精密单点定位，在学习函数之前，我们对运行ppp的处理选项设置进行了解
/* 自定义处理选项设置-PPP ----------------------------------------------------*/
prcopt.navsys = SYS_ALL;// GPS+GAL //SYS_ALL;
prcopt.mode = PMODE_PPP_STATIC;
prcopt.soltype = 0; // 求解类型(0:向前滤波,1:向后滤波,2:混合滤波)
prcopt.nf = 2; /* 参与计算的载波频率个数 (1:L1,2:L1+L2,3:L1+L2+L5) */
prcopt.navsys = SYS_GPS | SYS_CMP; // 处理的导航系统
prcopt.elmin = 10.0 * D2R;//卫星截止高度角
prcopt.sateph = EPHOPT_PREC;
prcopt.modear = 1; /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */
prcopt.elmin = 7 * D2R;
prcopt.ionoopt = IONOOPT_TEC;
prcopt.tropopt = TROPOPT_ESTG;
prcopt.tidecorr = 2; //地球潮汐改正选项(0:关闭,1:固体潮,2:固体潮+?+极移) 配置好prcopt结构体后就可以进入pppos函数了
/* precise point positioning -------------------------------------------------*/ extern void pppos(rtk_t* rtk, const obsd_t* obs, int n, const nav_t* nav)将处理选项设置信息存储到opt中，参数初始化
{ const prcopt_t* opt = &rtk->opt; double* rs, * dts, * var, * v, * H, * R, * azel, * xp, * Pp, dr[3] = { 0 }, std[3]; char str[32]; int i, j, nv, info, svh[MAXOBS], exc[MAXOBS] = { 0 }, stat = SOLQ_SINGLE; time2str(obs[0].time, str, 2); trace(3, "pppos : time=%s nx=%d n=%d\n", str, rtk->nx, n); rs = mat(6, n); dts = mat(2, n); var = mat(1, n); azel = zeros(2, n);调用udstate_ppp函数更新状态参数
for (i = 0; i < MAXSAT; i++) for (j = 0; j < opt->nf; j++) rtk->ssat[i].fix[j] = 0; /* temporal update of ekf states */ udstate_ppp(rtk, obs, n, nav);调用satposs函数计算卫星位置和钟差，testeclipse函数用于剔除有遮挡的卫星，
tidedisp函数用于计算固体潮、海洋潮汐以及极潮对接收机位置产生的影响/* satellite positions and clocks */ satposs(obs[0].time, obs, n, nav, rtk->opt.sateph, rs, dts, var, svh); /* exclude measurements of eclipsing satellite (block IIA) */ if (rtk->opt.posopt[3]) { testeclipse(obs, n, nav, rs); } /* earth tides correction */ if (opt->tidecorr) { tidedisp(gpst2utc(obs[0].time), rtk->x, opt->tidecorr == 1 ? 1 : 7, &nav->erp, opt->odisp[0], dr); }ppp_res函数用于计算各项残差，filter函数实现扩展卡尔曼滤波状态的量测更新
解算后，进行后验残差分析，符合限制则输出为精密单点定位状态，结束该迭代部分
for (i = 0; i < MAX_ITER; i++) { matcpy(xp, rtk->x, rtk->nx, 1); matcpy(Pp, rtk->P, rtk->nx, rtk->nx); /* prefit residuals */ if (!(nv = ppp_res(0, obs, n, rs, dts, var, svh, dr, exc, nav, xp, rtk, v, H, R, azel))) { trace(2, "%s ppp (%d) no valid obs data\n", str, i + 1); break; } /* measurement update of ekf states */ if ((info = filter(xp, Pp, H, v, R, rtk->nx, nv))) { trace(2, "%s ppp (%d) filter error info=%d\n", str, i + 1, info); break; } /* postfit residuals */ if (ppp_res(i + 1, obs, n, rs, dts, var, svh, dr, exc, nav, xp, rtk, v, H, R, azel)) { matcpy(rtk->x, xp, rtk->nx, 1); matcpy(rtk->P, Pp, rtk->nx, rtk->nx); stat = SOLQ_PPP; break; } }
2.2.1 udstate_ppp函数：更新状态参数
/* temporal update of states --------------------------------------------------*/
static void udstate_ppp(rtk_t* rtk, const obsd_t* obs, int n, const nav_t* nav)
{
trace(3, "udstate_ppp: n=%d\n", n);
/* temporal update of position 更新位置参数*/
udpos_ppp(rtk);
/* temporal update of clock 更新钟差*/
udclk_ppp(rtk);
/* temporal update of tropospheric parameters 更新对流层参数*/
if (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG) {
udtrop_ppp(rtk);
}
/* temporal update of ionospheric parameters 更新电离层参数*/
if (rtk->opt.ionoopt == IONOOPT_EST) {
udiono_ppp(rtk, obs, n, nav);
}
/* temporal update of L5-receiver-dcb parameters 更新码偏差参数*/
if (rtk->opt.nf >= 3) {
uddcb_ppp(rtk);
}
/* temporal update of phase-bias 更新相位偏差*/
udbias_ppp(rtk, obs, n, nav);
}2.2.1.1 udpos_ppp函数：更新位置参数
udpos_ppp函数根据不同的定位模式调用initx函数进行参数初始化
/* temporal update of position -----------------------------------------------*/
static void udpos_ppp(rtk_t* rtk)若为固定解模式，则只进行如下模块：
/* fixed mode */
if (rtk->opt.mode == PMODE_PPP_FIXED) {
for (i = 0; i < 3; i++) initx(rtk, rtk->opt.ru[i], 1E-8, i);
return;
}
若为静态PPP模式，只进行如下模块：
/* initialize position for first epoch */
if (norm(rtk->x, 3) <= 0.0) {
///PANLIN
for (i = 0; i < 3; i++) initx(rtk, rtk->sol.rr[i], VAR_POS, i);
//for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],SQR(0.02),i); ///only for ZTD estimation
///PANLIN
if (rtk->opt.dynamics) {
for (i = 3; i < 6; i++) initx(rtk, rtk->sol.rr[i], VAR_VEL, i);
for (i = 6; i < 9; i++) initx(rtk, 1E-6, VAR_ACC, i);
}
}
/* static ppp mode */
if (rtk->opt.mode == PMODE_PPP_STATIC) {
for (i = 0; i < 3; i++) {
rtk->P[i * (1 + rtk->nx)] += SQR(rtk->opt.prn[5]) * fabs(rtk->tt);
}//对协方差矩阵进行更新
return;
}在上面的代码中，当第一个历元的初始位置为0时会调用initx函数将之前的spp定位结果赋给状态初值，对协方差阵进行初始化，接收机位置的初始中误差VAR_POS=60、；若定位模式为动态，则还需初始化速度参数与其协方差阵，初始中误差VAR_VEL=60;加速度初VAR_ACC=60
在initx函数中使用了之前的spp结果对状态参数与其协方差阵进行了更新
/* initialize state and covariance -------------------------------------------*/ static void initx(rtk_t* rtk, double xi, double var, int i) { int j; rtk->x[i] = xi; for (j = 0; j < rtk->nx; j++) { rtk->P[i + j * rtk->nx] = rtk->P[j + i * rtk->nx] = i == j ? var : 0.0; } }在静态PPP模式时，解算每个历元前都需要进行位置协方差矩阵的更新
为动态定位模式 且没有运动的状态，即没有速度和加速度时，进行如下部分：
/* kinmatic mode without dynamics */
if (!rtk->opt.dynamics) {
for (i = 0; i < 3; i++) {
initx(rtk, rtk->sol.rr[i], VAR_POS, i);
}
return;
}为动态定位模式，继续进行如下部分：
/* generate valid state index */
ix = imat(rtk->nx, 1);
for (i = nx = 0; i < rtk->nx; i++) {
if (rtk->x[i] != 0.0 && rtk->P[i + i * rtk->nx] > 0.0) ix[nx++] = i;
}
if (nx < 9) {
free(ix);
return;
}可以看出，动态定位必须至少有9个参数才可以执行该部分，若动态模式下参数大于九个，则进行状态转移矩阵的构建，并进行状态矩阵和协方差矩阵的预测
为加速度部分添加过程噪声：
/* process noise added to only acceleration */
Q[0] = Q[4] = SQR(rtk->opt.prn[3]) * fabs(rtk->tt);
Q[8] = SQR(rtk->opt.prn[4]) * fabs(rtk->tt);
ecef2pos(rtk->x, pos);
covecef(pos, Q, Qv);
for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) {
rtk->P[i + 6 + (j + 6) * rtk->nx] += Qv[i + j * 3];
}其中用到两个函数，分别为
ecef2pos和covecef函数，分别用于将XYZ转为BLH以及站心地平坐标系下的协方差矩阵变成地心地固坐标系下的协方差矩阵
2.2.1.2 udclk_ppp函数：更新钟差参数
钟差的时间更新无论是静态还是动态模式，都是要进行的
/* temporal update of clock --------------------------------------------------*/
static void udclk_ppp(rtk_t* rtk)/* temporal update of clock --------------------------------------------------*/
static void udclk_ppp(rtk_t *rtk)
{
double dtr;
int i;
trace(3,"udclk_ppp:\n");
/* initialize every epoch for clock (white noise) */
for (i=0;i<NSYS;i++) {
if (rtk->opt.sateph==EPHOPT_PREC) {
/* time of prec ephemeris is based gpst */
/* negelect receiver inter-system bias */
dtr=rtk->sol.dtr[0];
}
else {
dtr=i==0?rtk->sol.dtr[0]:rtk->sol.dtr[0]+rtk->sol.dtr[i];
}
initx(rtk,CLIGHT*dtr,VAR_CLK,IC(i,&rtk->opt));
}
}
可以看到，在每个历元的钟差更新过程中，钟差的协方差都要置为0，方差都要初始化为3600
2.2.1.3 udtrop_ppp函数：更新对流层参数
2.2.1.4 udiono_ppp函数：更新电离层参数
2.2.1.5 uddcb_ppp函数：更新码偏差参数
2.2.1.6 udbias_ppp函数：更新相位偏差参数
2.2.2 satposs函数：计算卫星位置和钟差
http://t.csdnimg.cn/kHW9Nsatposs函数内容详见左侧链接
2.2.3 testeclipse函数：排除有遮挡的卫星
/* exclude meas of eclipsing satellite (block IIA) ---------------------------*/
static void testeclipse(const obsd_t* obs, int n, const nav_t* nav, double* rs)2.2.4 tidedisp函数：计算地球潮汐引起的位移
tidedisp函数计算固体潮海洋潮汐以及极潮对接收机位置产生的影响
/* tidal displacement ----------------------------------------------------------
* displacements by earth tides
* args : gtime_t tutc I time in utc
* double *rr I site position (ecef) (m)
* int opt I options (or of the followings)
* 1: solid earth tide
* 2: ocean tide loading
* 4: pole tide
* 8: elimate permanent deformation
* double *erp I earth rotation parameters (NULL: not used)
* double *odisp I ocean loading parameters (NULL: not used)
* odisp[0+i*6]: consituent i amplitude radial(m)
* odisp[1+i*6]: consituent i amplitude west (m)
* odisp[2+i*6]: consituent i amplitude south (m)
* odisp[3+i*6]: consituent i phase radial (deg)
* odisp[4+i*6]: consituent i phase west (deg)
* odisp[5+i*6]: consituent i phase south (deg)
* (i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1,
* 8:Mf,9:Mm,10:Ssa)
* double *dr O displacement by earth tides (ecef) (m)
* return : none
* notes : see ref [1], [2] chap 7
* see ref [4] 5.2.1, 5.2.2, 5.2.3
* ver.2.4.0 does not use ocean loading and pole tide corrections
*-----------------------------------------------------------------------------*/
extern void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
const double *odisp, double *dr)2.2.5 ppp_res函数：计算残差
/* phase and code residuals --------------------------------------------------*/
static int ppp_res(int post, const obsd_t* obs, int n, const double* rs,
const double* dts, const double* var_rs, const int* svh,
const double* dr, int* exc, const nav_t* nav,
const double* x, rtk_t* rtk, double* v, double* H, double* R,
double* azel)for (i = 0; i < n && i < MAXOBS; i++) //对H矩阵一列一列赋值调用geodist函数计算星地几何距离；调用satazel函数用于计算卫星的天顶距与方位角
if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 ||
satazel(pos, e, azel + i * 2) < opt->elmin) {
exc[i] = 1;
continue;
}对流层和电离层进行建模
/* tropospheric and ionospheric model */
if (!model_trop(obs[i].time, pos, azel + i * 2, opt, x, dtdx, nav, &dtrp, &vart) ||
!model_iono(obs[i].time, pos, azel + i * 2, opt, sat, x, nav, &dion, &vari)) {
continue;
}调用satantpcv函数用于计算卫星天线的相位中心变化；antmodel函数用于计算接收机天线相位中心的变化
/* satellite and receiver antenna model */
if (opt->posopt[0]) satantpcv(rs + i * 6, rr, nav->pcvs + sat - 1, dants);
antmodel(opt->pcvr, opt->antdel[0], azel + i * 2, opt->posopt[1], dantr);model_phw函数用于计算相位缠绕改正；corr_meas函数用于对码和相位观测值进行校正
/* phase windup model */
if (!model_phw(rtk->sol.time, sat, nav->pcvs[sat - 1].type,
opt->posopt[2] ? 2 : 0, rs + i * 6, rr, &rtk->ssat[sat - 1].phw)) {
continue;
}
/* corrected phase and code measurements */
corr_meas(obs + i, nav, azel + i * 2, &rtk->opt, dantr, dants,
rtk->ssat[sat - 1].phw, L, P, &Lc, &Pc);2.2.6 filter函数：实现扩展卡尔曼滤波
参数注释
/* kalman filter ---------------------------------------------------------------
* kalman filter state update as follows:
*
* K=P*H*(H'*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H')*P
*
* args : double *x I states vector (n x 1)参数向量
* double *P I covariance matrix of states (n x n)协方差阵
* double *H I transpose of design matrix (n x m)设计矩阵的转置
* double *v I innovation (measurement - model) (m x 1)信息向量
* double *R I covariance matrix of measurement error (m x m)量测噪声协方差阵
* int n,m I number of states and measurements
* double *xp O states vector after update (n x 1)预测参数
* double *Pp O covariance matrix of states after update (n x n)预测协方差
* return : status (0:ok,<0:error)
* notes : matirix stored by column-major order (fortran convention)
* if state x[i]==0.0, not updates state x[i]/P[i+i*n]
*-----------------------------------------------------------------------------*/
extern int filter(double *x, double *P, const double *H, const double *v,
const double *R, int n, int m)主程序
{
double *x_,*xp_,*P_,*Pp_,*H_;
int i,j,k,info,*ix;
ix=imat(n,1); for (i=k=0;i<n;i++) if (x[i]!=0.0&&P[i+i*n]>0.0) ix[k++]=i;//存储非零参数和正定协方差矩阵的行索引（将0值排除）
x_=mat(k,1); xp_=mat(k,1); P_=mat(k,k); Pp_=mat(k,k); H_=mat(k,m);//初始化矩阵
for (i=0;i<k;i++) {
x_[i]=x[ix[i]];
for (j=0;j<k;j++) P_[i+j*k]=P[ix[i]+ix[j]*n];
for (j=0;j<m;j++) H_[i+j*k]=H[ix[i]+j*n];
}//将传入的先验矩阵赋给这些矩阵
info=filter_(x_,P_,H_,v,R,k,m,xp_,Pp_);//进行矩阵运算
for (i=0;i<k;i++) {
x[ix[i]]=xp_[i];
for (j=0;j<k;j++) P[ix[i]+ix[j]*n]=Pp_[i+j*k];
}
free(ix); free(x_); free(xp_); free(P_); free(Pp_); free(H_);
return info;
}filter_函数是在传入的xp_和Pp_矩阵上对原矩阵进行计算。因此将结果赋给后验矩阵参数x,P
static int filter_(const double *x, const double *P, const double *H, const double *v, const double *R, int n, int m, double *xp, double *Pp) { double *F=mat(n,m),*Q=mat(m,m),*K=mat(n,m),*I=eye(n); int info; matcpy(Q,R,m,m); matcpy(xp,x,n,1); matmul("NN",n,m,n,1.0,P,H,0.0,F); /* Q=H'*P*H+R */ matmul("TN",m,m,n,1.0,H,F,1.0,Q); if (!(info=matinv(Q,m))) { matmul("NN",n,m,m,1.0,F,Q,0.0,K); /* K=P*H*Q^-1 */ matmul("NN",n,1,m,1.0,K,v,1.0,xp); /* xp=x+K*v */ matmul("NT",n,n,m,-1.0,K,H,1.0,I); /* Pp=(I-K*H')*P */ matmul("NN",n,n,n,1.0,I,P,0.0,Pp); } free(F); free(Q); free(K); free(I); return info; }
2.3 relpos函数：实现相对定位
relpos函数实现相对定位
/* relative positioning ------------------------------------------------------*/
static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
const nav_t *nav)处理过程
初始参数定义
{ prcopt_t *opt=&rtk->opt; gtime_t time=obs[0].time; double *rs,*dts,*var,*y,*e,*azel,*freq,*v,*H,*R,*xp,*Pp,*xa,*bias,dt; int i,j,f,n=nu+nr,ns,ny,nv,sat[MAXSAT],iu[MAXSAT],ir[MAXSAT],niter; int info,vflg[MAXOBS*NFREQ*2+1],svh[MAXOBS*2]; int stat=rtk->opt.mode<=PMODE_DGPS?SOLQ_DGPS:SOLQ_FLOAT; int nf=opt->ionoopt==IONOOPT_IFLC?1:opt->nf; trace(3,"relpos : nx=%d nu=%d nr=%d\n",rtk->nx,nu,nr); dt=timediff(time,obs[nu].time);//计算流动站和参考站时间差 rs=mat(6,n); dts=mat(2,n); var=mat(1,n); y=mat(nf*2,n); e=mat(3,n); azel=zeros(2,n); freq=zeros(nf,n);调用satsys函数判断卫星系统并存储到rtk->ssat[].sys中，将有效卫星标志赋值为0，信号强度赋值为零
for (i=0;i<MAXSAT;i++) { rtk->ssat[i].sys=satsys(i+1,NULL); for (j=0;j<NFREQ;j++) rtk->ssat[i].vsat[j]=0;/* vsat存储有效卫星标志信息 */ for (j=1;j<NFREQ;j++) rtk->ssat[i].snr [j]=0;/* snr存储信号强度dBHz信息 */ }·疑问，为什么snr字段从下标1开始赋值
调用satposs函数计算卫星的位置、钟差和频漂，这里的n为nu加nr
/* satellite positions/clocks */ satposs(time,obs,n,nav,opt->sateph,rs,dts,var,svh);//计算卫星位置、钟差和频漂调用zdres函数计算基站伪距和载波相位的非差残差
/* UD (undifferenced) residuals for base station */ if (!zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,var+nu,svh+nu,nav,rtk->rb,opt,1, y+nu*nf*2,e+nu*3,azel+nu*2,freq+nu*nf)) { errmsg(rtk,"initial base station position error\n"); free(rs); free(dts); free(var); free(y); free(e); free(azel); free(freq); return 0; }·传入的obs是obs+nu而不是obs是因为，obs是由移动站和基站的观测值共同组成，基站部分的在后面，所以加一个nu作为偏移。
·nu：移动站观测的卫星数 nr：参考站观测的卫星数
·rtk->rb是基站的位置信息，在rtkpos函数主程序中已计算出或在配置中给出在配置选项中设置，调用intpres函数对基站信息进行插值
/* time-interpolation of residuals (for post-processing) */ if (opt->intpref) { dt=intpres(time,obs+nu,nr,nav,rtk,y+nu*nf*2); }调用selsat函数选择基准站与参考站的公共卫星
/* select common satellites between rover and base-station */ if ((ns=selsat(obs,azel,nu,nr,opt,sat,iu,ir))<=0) { errmsg(rtk,"no common satellite\n"); free(rs); free(dts); free(var); free(y); free(e); free(azel); free(freq); return 0; }调用udstate函数更新状态时间
/* temporal update of states */ udstate(rtk,obs,sat,iu,ir,ns,nav);为变量申请内存并赋初值
xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx); xa=mat(rtk->nx,1); matcpy(xp,rtk->x,rtk->nx,1); ny=ns*nf*2+2; v=mat(ny,1); H=zeros(rtk->nx,ny); R=mat(ny,ny); bias=mat(rtk->nx,1);设置迭代次数
/* add 2 iterations for baseline-constraint moving-base */ niter=opt->niter+(opt->mode==PMODE_MOVEB&&opt->baseline[0]>0.0?2:0);·opt->niter为1，若为动基线模式且opt->baseline[0]>0.0则增加两次迭代次数
进行迭代
1、调用zdres函数计算流动站的非差残差结果
·此函数与上边计算基站的无差观测为同一个函数，只不过总体说来有两点不同：
·站点坐标使用的是xp,这个是kalman滤波时间更新中得到的新坐标，
·所有的输出，如ly,e,azel，起始存储位置为0，而上边的基准站无差观测时为nu∗size2、调用ddres函数进行双差残差及量测矩阵的更新
·输出的v,H,R都是卡尔曼滤波量测更新所必须的
·kf的量测向量是双差载波和双差伪距，这里为什么观测变成双差残差了呢？·残差/v=【参考星（移动站非差残差 - 基准站非差残差）- 非参考星（移动站非差残差 - 基准站非差残差）】-【双差模糊度】
·H = -参考星站星单位矢量 + 非参考星站星单位矢量；移动站下H参考星对应的列为1 ，非参考星对应的列为-1
保存伪距、载波残差；对新息阈值检测，卫星相关情况、相关值保存到ssat_t中 resc、resp、rejc、vsat3、调用filter函数实现KF量测更新
for (i=0;i<niter;i++) { /* UD (undifferenced) residuals for rover */ if (!zdres(0,obs,nu,rs,dts,var,svh,nav,xp,opt,0,y,e,azel,freq)) { errmsg(rtk,"rover initial position error\n"); stat=SOLQ_NONE; break; } /* DD (double-differenced) residuals and partial derivatives */ if ((nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,freq,iu,ir,ns,v,H,R, vflg))<1) { errmsg(rtk,"no double-differenced residual\n"); stat=SOLQ_NONE; break; } /* Kalman filter measurement update */ matcpy(Pp,rtk->P,rtk->nx,rtk->nx); if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) { errmsg(rtk,"filter error (info=%d)\n",info); stat=SOLQ_NONE; break; } trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR(opt),13,4); }量测更新完成，检验结果，若有效则利用更新后的结果调用zdres函数计算残差
·利用浮点解调用ddres函数计算双差残差及量测噪声
·调用valpos函数对方差R和量测值vflg进行检验
·存储浮点解
·存储模糊度相关的信息，统计有效卫星个数
·检测卫星数量是否有效
if (stat!=SOLQ_NONE&&zdres(0,obs,nu,rs,dts,var,svh,nav,xp,opt,0,y,e,azel, freq)) { /* post-fit residuals for float solution 利用浮点解计算双差残差及量测噪声*/ nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,freq,iu,ir,ns,v,NULL,R,vflg); /* validation of float solution */ if (valpos(rtk,v,R,vflg,nv,4.0)) { /* update state and covariance matrix */ matcpy(rtk->x,xp,rtk->nx,1); matcpy(rtk->P,Pp,rtk->nx,rtk->nx); /* update ambiguity control struct */ rtk->sol.ns=0; for (i=0;i<ns;i++) for (f=0;f<nf;f++) { if (!rtk->ssat[sat[i]-1].vsat[f]) continue; rtk->ssat[sat[i]-1].lock[f]++; rtk->ssat[sat[i]-1].outc[f]=0; if (f==0) rtk->sol.ns++; /* valid satellite count by L1 */ } /* lack of valid satellites */ if (rtk->sol.ns<4) stat=SOLQ_NONE; } else stat=SOLQ_NONE; }调用resamb_LAMBDA函数进行整周模糊度解算
/* resolve integer ambiguity by LAMBDA */ if (stat!=SOLQ_NONE&&resamb_LAMBDA(rtk,bias,xa)>1) { //模糊度解算成功，根据固定解计算双差残差和协方差并进行校验 if (zdres(0,obs,nu,rs,dts,var,svh,nav,xa,opt,0,y,e,azel,freq)) { /* post-fit reisiduals for fixed solution */ nv=ddres(rtk,nav,dt,xa,NULL,sat,y,e,azel,freq,iu,ir,ns,v,NULL,R, vflg); /* validation of fixed solution */ if (valpos(rtk,v,R,vflg,nv,4.0)) { /* hold integer ambiguity */ //固定解验证有效，若配置为hold模式，需要存储模糊度信息 if (++rtk->nfix>=rtk->opt.minfix&& rtk->opt.modear==ARMODE_FIXHOLD) { holdamb(rtk,xa); } stat=SOLQ_FIX; } } }存储rtk结果，位置和速度以及方差信息（若状态为固定解，存储固定解结果）
/* save solution status */ if (stat==SOLQ_FIX) { for (i=0;i<3;i++) { rtk->sol.rr[i]=rtk->xa[i]; rtk->sol.qr[i]=(float)rtk->Pa[i+i*rtk->na]; } rtk->sol.qr[3]=(float)rtk->Pa[1]; rtk->sol.qr[4]=(float)rtk->Pa[1+2*rtk->na]; rtk->sol.qr[5]=(float)rtk->Pa[2]; if (rtk->opt.dynamics) { /* velocity and covariance */ for (i=3;i<6;i++) { rtk->sol.rr[i]=rtk->xa[i]; rtk->sol.qv[i-3]=(float)rtk->Pa[i+i*rtk->na]; } rtk->sol.qv[3]=(float)rtk->Pa[4+3*rtk->na]; rtk->sol.qv[4]=(float)rtk->Pa[5+4*rtk->na]; rtk->sol.qv[5]=(float)rtk->Pa[5+3*rtk->na]; } } else { for (i=0;i<3;i++) { rtk->sol.rr[i]=rtk->x[i]; rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx]; } rtk->sol.qr[3]=(float)rtk->P[1]; rtk->sol.qr[4]=(float)rtk->P[1+2*rtk->nx]; rtk->sol.qr[5]=(float)rtk->P[2]; if (rtk->opt.dynamics) { /* velocity and covariance */ for (i=3;i<6;i++) { rtk->sol.rr[i]=rtk->x[i]; rtk->sol.qv[i-3]=(float)rtk->P[i+i*rtk->nx]; } rtk->sol.qv[3]=(float)rtk->P[4+3*rtk->nx]; rtk->sol.qv[4]=(float)rtk->P[5+4*rtk->nx]; rtk->sol.qv[5]=(float)rtk->P[5+3*rtk->nx]; } rtk->nfix=0; }存储当前历元的载波相位信息，供下次使用
for (i=0;i<n;i++) for (j=0;j<nf;j++) { if (obs[i].L[j]==0.0) continue; rtk->ssat[obs[i].sat-1].pt[obs[i].rcv-1][j]=obs[i].time; rtk->ssat[obs[i].sat-1].ph[obs[i].rcv-1][j]=obs[i].L[j]; }存储SNR信息
for (i=0;i<ns;i++) for (j=0;j<nf;j++) { /* output snr of rover receiver */ rtk->ssat[sat[i]-1].snr[j]=obs[iu[i]].SNR[j]; }更新卫星的fix信息以及周跳信息
for (i=0;i<MAXSAT;i++) for (j=0;j<nf;j++) { if (rtk->ssat[i].fix[j]==2&&stat!=SOLQ_FIX) rtk->ssat[i].fix[j]=1; if (rtk->ssat[i].slip[j]&1) rtk->ssat[i].slipc[j]++; } //释放局部变量，返回定位状态 free(rs); free(dts); free(var); free(y); free(e); free(azel); free(freq); free(xp); free(Pp); free(xa); free(v); free(H); free(R); free(bias); if (stat!=SOLQ_NONE) rtk->sol.stat=stat; return stat!=SOLQ_NONE; }
2.3.1 satsys函数:确定卫星系统及其PRN编号
/* satellite number to satellite system ----------------------------------------
* convert satellite number to satellite system
* args : int sat I satellite number (1-MAXSAT)
* int *prn IO satellite prn/slot number (NULL: no output)
* return : satellite system (SYS_GPS,SYS_GLO,...)
*-----------------------------------------------------------------------------*/
extern int satsys(int sat, int *prn)2.3.2 satposs函数：计算每颗卫星的位置和速度（rs）、钟差和频漂（dts）
satposs函数内容详见http://t.csdnimg.cn/nIySm
228
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
