RTKLIB相对定位部分算法梳理

前言
一、相对定位整体框架
二、伪距单点定位
- 1、卫星位置
- 2、伪距定位
- - 伪距残差
  - 电离层延时校正
  - 对流层延时校正
  - 最小二乘
  - 验证位置解
- 3、接收机自主正直性检测
- 4、多普勒定速
三、载波相位相对定位
- 1、非差残差
- 2、卡尔曼滤波时间更新
- - 位置更新
  - 单差整周模糊度更新
  - - 周跳检测
    - 单差模糊度更新
- 3、双差残差及其偏导
- 4、卡尔曼滤波量测更新
- 5、验证解的有效性
- 6、固定整周模糊度

前言

RTKLIB是一个著名的GNSS开源项目，里面涵盖了GNSS的方方面面，相信很多学习GNSS的朋友读的第一份源码便是RTKLIB。恰恰因为RTKLIB涵盖的东西太多了，学习起来比较麻烦，博主整理了RTKLIB的相对定位部分算法，方便自己回顾复习，也供大家参考学习。本博客主要参考RTKLIB源码，RTKLIB手册，《GPS测量与数据处理》，及多篇RTKLIB相关博客。

一、相对定位整体框架

二、伪距单点定位

1、卫星位置

2、伪距定位

伪距残差

/* pseudorange residuals -----------------------------------------------------*/
static int rescode(int iter, const obsd_t *obs, int n, const double *rs,const double *dts, const double *vare, const int *svh,const nav_t *nav, const double *x, const prcopt_t *opt,double *v, double *H, double *var, double *azel, int *vsat,double *resp, int *ns)
{double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1;int i,j,nv=0,sys,mask[4]={0};trace(3,"resprng : n=%d\n",n);for (i=0;i<3;i++) rr[i]=x[i];dtr=x[3];ecef2pos(rr,pos);for (i=*ns=0;i<n&&i<MAXOBS;i++) {vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;if (!(sys=satsys(obs[i].sat,NULL))) continue;/* reject duplicated observation data 剔除重复观测数据 */if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) {trace(2,"duplicated observation data %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat);i++;continue;}/* geometric distance/azimuth/elevation angle 几何距离、方位、仰角 */if ((r=geodist(rs+i*6,rr,e))<=0.0||satazel(pos,e,azel+i*2)<opt->elmin) continue;/* psudorange with code bias correction 码偏差校正后的伪距 */if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue;/* excluded satellite? */if (satexclude(obs[i].sat,vare[i],svh[i],opt)) continue;/* ionospheric corrections */if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2,iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) continue;/* GPS-L1 -> L1/B1 */if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) {dion*=SQR(lam_L1/lam_carr[0]);}/* tropospheric corrections */if (!tropcorr(obs[i].time,nav,pos,azel+i*2,iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {continue;}/* pseudorange residual 伪距残差 */v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);/* design matrix 设计矩阵 */for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0);/* time system and receiver bias offset correction 时间系统与接收机偏移补偿校正 */if      (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;}else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;}else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}else mask[0]=1;vsat[i]=1; resp[i]=v[nv]; (*ns)++;/* error variance 伪距测量量误差方差 */var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;trace(4,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n",obs[i].sat,azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1]));}/* constraint to avoid rank-deficient 避免秩不足的约束 */for (i=0;i<4;i++) {if (mask[i]) continue;v[nv]=0.0;for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;var[nv++]=0.01;}return nv;
}

电离层延时校正

广播电离层模型（Klobuchar）：
卫星广播星历中包含如下广播电离层参数：

利用这些参数及接收机位置、卫星仰角方位角等，L1的电离层延时I(m)可以利用如下公式计算得到：

对应代码：

/* ionosphere model ------------------------------------------------------------
* compute ionospheric delay by broadcast ionosphere model (klobuchar model)
* args   : gtime_t t        I   time (gpst)
*          double *ion      I   iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3}
*          double *pos      I   receiver position {lat,lon,h} (rad,m)
*          double *azel     I   azimuth/elevation angle {az,el} (rad)
* return : ionospheric delay (L1) (m)
*-----------------------------------------------------------------------------*/
extern double ionmodel(gtime_t t, const double *ion, const double *pos,const double *azel)
{const double ion_default[]={ /* 2004/1/1 */0.1118E-07,-0.7451E-08,-0.5961E-07, 0.1192E-06,0.1167E+06,-0.2294E+06,-0.1311E+06, 0.1049E+07};double tt,f,psi,phi,lam,amp,per,x;int week;if (pos[2]<-1E3||azel[1]<=0) return 0.0;if (norm(ion,8)<=0.0) ion=ion_default;/* earth centered angle (semi-circle) */psi=0.0137/(azel[1]/PI+0.11)-0.022;/* subionospheric latitude/longitude (semi-circle) */phi=pos[0]/PI+psi*cos(azel[0]);if      (phi> 0.416) phi= 0.416;else if (phi<-0.416) phi=-0.416;lam=pos[1]/PI+psi*sin(azel[0])/cos(phi*PI);/* geomagnetic latitude (semi-circle) */phi+=0.064*cos((lam-1.617)*PI);/* local time (s) */tt=43200.0*lam+time2gpst(t,&week);tt-=floor(tt/86400.0)*86400.0; /* 0<=tt<86400 *//* slant factor */f=1.0+16.0*pow(0.53-azel[1]/PI,3.0);/* ionospheric delay */amp=ion[0]+phi*(ion[1]+phi*(ion[2]+phi*ion[3]));per=ion[4]+phi*(ion[5]+phi*(ion[6]+phi*ion[7]));amp=amp<    0.0?    0.0:amp;per=per<72000.0?72000.0:per;x=2.0*PI*(tt-50400.0)/per;return CLIGHT*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9);
}

对流层延时校正

对流层模型（Saastamoinen）：
标准大气模型（大气压力、大气温度、水汽压力）：

对流层延时T可以通过上面的参数及卫星高度角计算得到：

对应代码：

/* troposphere model -----------------------------------------------------------
* compute tropospheric delay by standard atmosphere and saastamoinen model
* args   : gtime_t time     I   time
*          double *pos      I   receiver position {lat,lon,h} (rad,m)
*          double *azel     I   azimuth/elevation angle {az,el} (rad)
*          double humi      I   relative humidity
* return : tropospheric delay (m)
*-----------------------------------------------------------------------------*/
extern double tropmodel(gtime_t time, const double *pos, const double *azel,double humi)
{const double temp0=15.0; /* temparature at sea level */double hgt,pres,temp,e,z,trph,trpw;if (pos[2]<-100.0||1E4<pos[2]||azel[1]<=0) return 0.0;/* standard atmosphere */hgt=pos[2]<0.0?0.0:pos[2];pres=1013.25*pow(1.0-2.2557E-5*hgt,5.2568);temp=temp0-6.5E-3*hgt+273.16;e=6.108*humi*exp((17.15*temp-4684.0)/(temp-38.45));/* saastamoninen model */z=PI/2.0-azel[1];trph=0.0022768*pres/(1.0-0.00266*cos(2.0*pos[0])-0.00028*hgt/1E3)/cos(z);trpw=0.002277*(1255.0/temp+0.05)*e/cos(z);return trph+trpw;
}

最小二乘

高斯牛顿迭代非线性最小二乘：

最小二乘各项矩阵构造：

对应代码：

/* least square estimation -----------------------------------------------------
* least square estimation by solving normal equation (x=(A*A')^-1*A*y)
* args   : double *A        I   transpose of (weighted) design matrix (n x m)
*          double *y        I   (weighted) measurements (m x 1)
*          int    n,m       I   number of parameters and measurements (n<=m)
*          double *x        O   estmated parameters (n x 1)
*          double *Q        O   esimated parameters covariance matrix (n x n)
* return : status (0:ok,0>:error)
* notes  : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2))
*          matirix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
extern int lsq(const double *A, const double *y, int n, int m, double *x,double *Q)
{double *Ay;int info;if (m<n) return -1;Ay=mat(n,1);matmul("NN",n,1,m,1.0,A,y,0.0,Ay); /* Ay=A*y */matmul("NT",n,n,m,1.0,A,A,0.0,Q);  /* Q=A*A' */if (!(info=matinv(Q,n))) matmul("NN",n,1,n,1.0,Q,Ay,0.0,x); /* x=Q^-1*Ay */free(Ay);return info;
}

验证位置解

卡方检测+GDOP检测：

/* validate solution ---------------------------------------------------------*/
static int valsol(const double *azel, const int *vsat, int n,const prcopt_t *opt, const double *v, int nv, int nx,char *msg)
{double azels[MAXOBS*2],dop[4],vv;int i,ns;trace(3,"valsol  : n=%d nv=%d\n",n,nv);/* chi-square validation of residuals */vv=dot(v,v,nv);if (nv>nx&&vv>chisqr[nv-nx-1]) {sprintf(msg,"chi-square error nv=%d vv=%.1f cs=%.1f",nv,vv,chisqr[nv-nx-1]);return 0;}/* large gdop check */for (i=ns=0;i<n;i++) {if (!vsat[i]) continue;azels[  ns*2]=azel[  i*2];azels[1+ns*2]=azel[1+i*2];ns++;}dops(ns,azels,opt->elmin,dop);if (dop[0]<=0.0||dop[0]>opt->maxgdop) {sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]);return 0;}return 1;
}

3、接收机自主正直性检测

/* raim fde (failure detection and exclution) -------------------------------*/
static int raim_fde(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, sol_t *sol,double *azel, int *vsat, double *resp, char *msg)
{obsd_t *obs_e;sol_t sol_e={{0}};char tstr[32],name[16],msg_e[128]=" ";double *rs_e,*dts_e,*vare_e,*azel_e,*resp_e,rms_e,rms=100.0;int i,j,k,nvsat,stat=0,*svh_e,*vsat_e,sat=0;trace(3,"raim_fde: %s n=%2d\n",time_str(obs[0].time,0),n);if (!(obs_e=(obsd_t *)malloc(sizeof(obsd_t)*n))) return 0;rs_e = mat(6,n); dts_e = mat(2,n); vare_e=mat(1,n); azel_e=zeros(2,n);svh_e=imat(1,n); vsat_e=imat(1,n); resp_e=mat(1,n); for (i=0;i<n;i++) {/* satellite exclution */for (j=k=0;j<n;j++) {if (j==i) continue;obs_e[k]=obs[j];matcpy(rs_e +6*k,rs +6*j,6,1);matcpy(dts_e+2*k,dts+2*j,2,1);vare_e[k]=vare[j];svh_e[k++]=svh[j];}/* estimate receiver position without a satellite */if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,&sol_e,azel_e,vsat_e,resp_e,msg_e)) {trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg);continue;}for (j=nvsat=0,rms_e=0.0;j<n-1;j++) {if (!vsat_e[j]) continue;rms_e+=SQR(resp_e[j]);nvsat++;}if (nvsat<5) {trace(3,"raim_fde: exsat=%2d lack of satellites nvsat=%2d\n",obs[i].sat,nvsat);continue;}rms_e=sqrt(rms_e/nvsat);trace(3,"raim_fde: exsat=%2d rms=%8.3f\n",obs[i].sat,rms_e);if (rms_e>rms) continue;/* save result */for (j=k=0;j<n;j++) {if (j==i) continue;matcpy(azel+2*j,azel_e+2*k,2,1);vsat[j]=vsat_e[k];resp[j]=resp_e[k++];}stat=1;*sol=sol_e;sat=obs[i].sat;rms=rms_e;vsat[i]=0;strcpy(msg,msg_e);}if (stat) {time2str(obs[0].time,tstr,2); satno2id(sat,name);trace(2,"%s: %s excluded by raim\n",tstr+11,name);}free(obs_e);free(rs_e ); free(dts_e ); free(vare_e); free(azel_e);free(svh_e); free(vsat_e); free(resp_e);return stat;
}

4、多普勒定速

最小二乘各项矩阵构造：

三、载波相位相对定位

1、非差残差

/* undifferenced phase/code residuals ----------------------------------------*/
/*args:  I   base:           1=base,0=roverI   obs:             sat observationsI   n:               # of satsI   rs [(0:2)+i*6]: sat position {x,y,z} (m)I   dts[(0:1)+i*2]: sat clock {bias,drift} (s|s/s)I   svh:            sat health flagsI   nav:            sat nav dataI   rr:             receiver pos (x,y,z)I   opt:            optionsI   index:          0=base,1=roverO   y[(0:1)+i*2]:   zero diff residuals {phase,code} (m)O   e:              line of sight unit vectors to sats(卫星观测方向单位矢量)O   azel:           [az, el] to sats （方位角、仰角）
return:
int                  O     (1:ok,0:error)
计算某历元所有卫星（无差的）载波相位/伪距残差(残差 = 观测量 - 伪距估计量)
*/
static int zdres(int base, const obsd_t *obs, int n, const double *rs,const double *dts, const double *var, const int *svh,const nav_t *nav, const double *rr, const prcopt_t *opt, //默认的index=1；n基准站卫星数；int index, double *y, double *e, double *azel)
{double r,rr_[3],pos[3],dant[NFREQ]={0},disp[3];double zhd,zazel[]={0.0,90.0*D2R};int i,nf=NF(opt);trace(3,"zdres   : n=%d\n",n);for (i=0;i<n*nf*2;i++) y[i]=0.0; //将y数组中的所有元素初始化为0；if (norm(rr,3)<=0.0) return 0; /* no receiver position 如果没有接收机位置，返回0。这里传进来的是伪距单点定位的结果，如果是计算基站的残差，这里的位置输入参数是基站位置，如果计算移动站，位置输入参数是kalman滤波中的位置状态量；*/for (i=0;i<3;i++) rr_[i]=rr[i];//给rr_赋值接收机位置坐标/* earth tide correction 地球潮汐改正，如果在配置中选择进行潮汐误差修正，调用tidedisp函数，对接收机位置进行地球潮汐误差项的修正；一般没考虑计算*/if (opt->tidecorr) {tidedisp(gpst2utc(obs[0].time),rr_,opt->tidecorr,&nav->erp,opt->odisp[base],disp);for (i=0;i<3;i++) rr_[i]+=disp[i];}ecef2pos(rr_,pos); //基准站的ecef坐标（xyz）转换为大地坐标（经纬高）存放posfor (i=0;i<n;i++) { //逐卫星计算/* compute geometric-range and azimuth/elevation angle 调用geodist函数，计算接收机到所有卫星的几何距离（伪距估计量）、单位观测矢量；然后调用satazel函数得到卫星方位角、仰角；*/if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue; //基准站和卫星几何距离包括了萨格纳克效应改正if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue; //站星方位角+高度角/* excluded satellite? 通过satexclude函数判断该卫星是否需要被排除； */if (satexclude(obs[i].sat,var[i],svh[i],opt)) continue;/* satellite clock-bias 计算出的几何距离进行钟差改正r+=钟差 */r+=-CLIGHT*dts[i*2];/* troposphere delay model (hydrostatic) */zhd=tropmodel(obs[0].time,pos,zazel,0.0); //计算大地坐标下对流层延迟r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd; //几何距离进行对流层延迟改正r+=对流层改正； tropmapf为投影函数/* receiver antenna phase center correction 调用antmodel函数，计算接收机天线偏移量；计算接收机天线相位中心改正dant[]；range offsets for each frequency */antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1],dant);/* undifferenced phase/code residual for satellite 调用zdres_sat计算每颗卫星的载波相位/伪距残差，存放在y数组中。在卫星端计算非差相位残差，分两种情况1电离层组合，2非组合 依据得到的天线改正值dant和经过改正的几何距离r与原来的伪距和相位观测值作差得到残差。*/zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2);}trace(4,"rr_=%.3f %.3f %.3f\n",rr_[0],rr_[1],rr_[2]);trace(4,"pos=%.9f %.9f %.3f\n",pos[0]*R2D,pos[1]*R2D,pos[2]);for (i=0;i<n;i++) {trace(4,"sat=%2d %13.3f %13.3f %13.3f %13.10f %6.1f %5.1f\n",obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],dts[i*2],azel[i*2]*R2D,azel[1+i*2]*R2D);}trace(4,"y=\n"); tracemat(4,y,nf*2,n,13,3);return 1;
}

2、卡尔曼滤波时间更新

/* temporal update of states --------------------------------------------------*/
/* args:  IO  rtk_t *rtk:      rtk solution structureI   obsd_t *obs:     sat observationsI   int *sat:        移动站、基站共视星列表I   int *iu          移动站共视星所在obs数组中的索引I   int *ir          基站共视星所在obs数组中的索引I   int ns:          共视星个数I   nav_t *nav:      卫星导航电文首先对位置、速度、加速度状态量做时间更新；其次对对流层参数做时间更新；对单差相位偏移状态量进行时间更新。
*/
static void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat,const int *iu, const int *ir, int ns, const nav_t *nav)
{double tt=rtk->tt,bl,dr[3];trace(3,"udstate : ns=%d\n",ns);/* temporal update of position/velocity/acceleration */udpos(rtk,tt);/* temporal update of ionospheric parameters */if (rtk->opt.ionoopt>=IONOOPT_EST) {bl=baseline(rtk->x,rtk->rb,dr);udion(rtk,tt,bl,sat,ns);}/* temporal update of tropospheric parameters */if (rtk->opt.tropopt>=TROPOPT_EST) {udtrop(rtk,tt,bl);}/* temporal update of eceiver h/w bias */if (rtk->opt.glomodear==2&&(rtk->opt.navsys&SYS_GLO)) {udrcvbias(rtk,tt);}/* temporal update of phase-bias */if (rtk->opt.mode>PMODE_DGPS) {udbias(rtk,tt,obs,sat,iu,ir,ns,nav);}
}

位置更新

/* temporal update of position/velocity/acceleration -------------------------*/
/*args:  IO  rtk_t *rtk:      rtk solution structureI   double tt:       当前历元与前一历元的时间差*/
static void udpos(rtk_t *rtk, double tt)
{double *F,*P,*FP,*x,*xp,pos[3],Q[9]={0},Qv[9],var=0.0;int i,j,*ix,nx;trace(3,"udpos   : tt=%.3f\n",tt);/* fixed mode 移动站固定 如果现在的定位模式是PMODE_FIXED的状态，即移动站固定的定位模式，则调用initx函数，利用移动站的位置对前三个状态（即位置）进行初始化，将并其在协方差阵P的方差设置为一个较小的方差值（1E-8）。*/if (rtk->opt.mode==PMODE_FIXED) {for (i=0;i<3;i++) initx(rtk,rtk->opt.ru[i],1E-8,i);return;}/* initialize position for first epoch        对第一个历元时刻的位置、速度、加速度状态量进行初始化，其中位置状态量初始化为单点定位所得到的位置值，方差设置为VAR_POS（60）。如果在配置中选择了将dynamics打开，则将速度状态量初始化为单点定位的速度值，方差设置为VAR_VEL（10），将加速度状态量初始化1E-6，方差设置为VAR_ACC（10）。*/if (norm(rtk->x,3)<=0.0) {for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i);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 mode */if (rtk->opt.mode==PMODE_STATIC) return;/* kinmatic mode without dynamics        如果配置中没有打开Rec dynamics，每次状态更新都将位置状态量赋值为单点定位的位置，方差设置为VAR_POS（60）。*/if (!rtk->opt.dynamics) {for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i);return;}/* check variance of estimated postion 检查位置状态量的协方差，如果位置状态量的平均方差大于VAR_POS（60），则将位置状态量、速度状态量重置为单点定位值，加速度状态量重置为1E-6。根据3我们可以知道，这一步主要是检查dynamics打开时的情况。*/for (i=0;i<3;i++) var+=rtk->P[i+i*rtk->nx];var/=3.0;if (var>VAR_POS) {/* reset position with large variance */for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i);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);trace(2,"reset rtk position due to large variance: var=%.3f\n",var);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;}/* state transition of position/velocity/acceleration        对位置、速度、加速度状态量按照卡尔曼滤波方程进行一步预测，即时间更新。实际上速度、位置、加速度之间的状态方程非常简单，*/F=eye(nx); P=mat(nx,nx); FP=mat(nx,nx); x=mat(nx,1); xp=mat(nx,1);for (i=0;i<6;i++) {F[i+(i+3)*nx]=tt;}for (i=0;i<3;i++) {F[i+(i+6)*nx]=SQR(tt)/2.0;}for (i=0;i<nx;i++) {x[i]=rtk->x[ix[i]];for (j=0;j<nx;j++) {P[i+j*nx]=rtk->P[ix[i]+ix[j]*rtk->nx];}}/* x=F*x, P=F*P*F+Q */matmul("NN",nx,1,nx,1.0,F,x,0.0,xp);matmul("NN",nx,nx,nx,1.0,F,P,0.0,FP);matmul("NT",nx,nx,nx,1.0,FP,F,0.0,P);for (i=0;i<nx;i++) {rtk->x[ix[i]]=xp[i];for (j=0;j<nx;j++) {rtk->P[ix[i]+ix[j]*rtk->nx]=P[i+j*nx];}}/* process noise added to only acceleration 该过程噪声建模为随机游走噪声。由于在配置中所设的“Receiver Accel horiz/vertical”是东北天坐标系下的，因此还需要将该过程噪声的方差阵由东北天坐标系转到地球固定坐标系。由此，便完成了位置、速度、加速度状态量的时间更新。*/Q[0]=Q[4]=SQR(rtk->opt.prn[3])*fabs(tt);Q[8]=SQR(rtk->opt.prn[4])*fabs(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];}free(ix); free(F); free(P); free(FP); free(x); free(xp);
}

单差整周模糊度更新

/* temporal update of phase biases -------------------------------------------*/
/*args:  IO  rtk_t *rtk:  rtk solution structureI   double tt:       当前历元与前一历元的时间差I   obsd_t *obs:     sat observationsI   int *sat:        移动站、基站共视星列表I   int *iu          移动站共视星所在obs数组中的索引I   int *ir          基站共视星所在obs数组中的索引I   int ns:          共视星个数I   nav_t *nav:      卫星导航电文对单差整周模糊度状态量进行时间更新在整个单差相位偏移状态量的时间更新中，实际上仅对单差相位偏移状态量为0的那些卫星进行了初始化，其余卫星的单差相位偏移状态量并没有变化，仅仅是在协方差阵中加入了过程噪声。
*/
static void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,const int *iu, const int *ir, int ns, const nav_t *nav)
{double cp,pr,cp1,cp2,pr1,pr2,*bias,offset,lami,lam1,lam2,C1,C2;int i,j,f,slip,reset,nf=NF(&rtk->opt);trace(3,"udbias  : tt=%.3f ns=%d\n",tt,ns);/*首先是对所有共视星进行周跳检测：调用了detslp_ll函数，根据LLI来判断基站和移动站周跳；然后调用detslp_gf_L1L2、detslp_gf_L1L5函数，利用几何无关组合进行周跳检测；最后调用detslp_dop函数，利用多普勒进行周跳检测（但该函数由于时间跳变的原因，暂未使用）。 */for (i=0;i<ns;i++) {/* detect cycle slip by LLI */for (f=0;f<rtk->opt.nf;f++) rtk->ssat[sat[i]-1].slip[f]&=0xFC;detslp_ll(rtk,obs,iu[i],1);detslp_ll(rtk,obs,ir[i],2);/* detect cycle slip by geometry-free phase jump */detslp_gf_L1L2(rtk,obs,iu[i],ir[i],nav);detslp_gf_L1L5(rtk,obs,iu[i],ir[i],nav);/* detect cycle slip by doppler and phase difference */detslp_dop(rtk,obs,iu[i],1,nav);detslp_dop(rtk,obs,ir[i],2,nav);/* update half-cycle valid flag */for (f=0;f<nf;f++) {rtk->ssat[sat[i]-1].half[f]=!((obs[iu[i]].LLI[f]&2)||(obs[ir[i]].LLI[f]&2));}}for (f=0;f<nf;f++) {/*对所有卫星进行循环，判断是否需要重置单差相位偏移状态量。如果所配置的AR的模式为instantaneous，或者卫星载波相位的中断次数大于配置中所设置的最大次数，则将单差相位偏移状态量重置为0。*//* reset phase-bias if instantaneous AR or expire obs outage counter */for (i=1;i<=MAXSAT;i++) {reset=++rtk->ssat[i-1].outc[f]>(unsigned int)rtk->opt.maxout;if (rtk->opt.modear==ARMODE_INST&&rtk->x[IB(i,f,&rtk->opt)]!=0.0) {initx(rtk,0.0,0.0,IB(i,f,&rtk->opt));}else if (reset&&rtk->x[IB(i,f,&rtk->opt)]!=0.0) {initx(rtk,0.0,0.0,IB(i,f,&rtk->opt));trace(3,"udbias : obs outage counter overflow (sat=%3d L%d n=%d)\n",i,f+1,rtk->ssat[i-1].outc[f]);rtk->ssat[i-1].outc[f]=0;}if (rtk->opt.modear!=ARMODE_INST&&reset) {rtk->ssat[i-1].lock[f]=-rtk->opt.minlock;}}/* reset phase-bias if detecting cycle slip 对共视星进行循环，对单差相位偏移状态量的协方差阵加入过程噪声，如果发现有周跳或者异常值，则单差相位偏移状态量重置为0。*/for (i=0;i<ns;i++) {j=IB(sat[i],f,&rtk->opt);rtk->P[j+j*rtk->nx]+=rtk->opt.prn[0]*rtk->opt.prn[0]*fabs(tt);slip=rtk->ssat[sat[i]-1].slip[f];if (rtk->opt.ionoopt==IONOOPT_IFLC) slip|=rtk->ssat[sat[i]-1].slip[1];if (rtk->opt.modear==ARMODE_INST||!(slip&1)) continue;rtk->x[j]=0.0;rtk->ssat[sat[i]-1].lock[f]=-rtk->opt.minlock;}bias=zeros(ns,1);/* estimate approximate phase-bias by phase - code 对共视星进行循环，利用“单差伪距”和“单差载波相位”计算一个“单差相位偏移”估计值，来对单差相位偏移状态量进行更新。由于如果忽略伪距误差，那么伪距减去载波相位，则应该是载波相位（m）的相位偏移(m)，所以这里计算的是一个大致的相位偏移bias[i]。如果配置为无电离层组合，计算则按照无电离层组合的方式来计算。最后，仅计算所有有效星（单差相位偏移状态不为0）的offset = sum of (bias - phase-bias)。其实就是计算每颗有效星bias[i]与单单差相位偏移状态量的偏差，然后把所有有效星的这个偏差值加起来，之后会除以有效星的个数，最终就是求一个偏差平均值rtk->com_bias。*/for (i=j=0,offset=0.0;i<ns;i++) {if (rtk->opt.ionoopt!=IONOOPT_IFLC) {cp=sdobs(obs,iu[i],ir[i],f); /* cycle 载波相位单差观测值                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              */pr=sdobs(obs,iu[i],ir[i],f+NFREQ);/* 伪距单差观测值 */lami=nav->lam[sat[i]-1][f];if (cp==0.0||pr==0.0||lami<=0.0) continue;bias[i]=cp-pr/lami;//大致的相位偏移}else {cp1=sdobs(obs,iu[i],ir[i],0);cp2=sdobs(obs,iu[i],ir[i],1);pr1=sdobs(obs,iu[i],ir[i],NFREQ);pr2=sdobs(obs,iu[i],ir[i],NFREQ+1);lam1=nav->lam[sat[i]-1][0];lam2=nav->lam[sat[i]-1][1];if (cp1==0.0||cp2==0.0||pr1==0.0||pr2==0.0||lam1<=0.0||lam2<=0.0) continue;C1= SQR(lam2)/(SQR(lam2)-SQR(lam1));C2=-SQR(lam1)/(SQR(lam2)-SQR(lam1));bias[i]=(C1*lam1*cp1+C2*lam2*cp2)-(C1*pr1+C2*pr2);}if (rtk->x[IB(sat[i],f,&rtk->opt)]!=0.0) {offset+=bias[i]-rtk->x[IB(sat[i],f,&rtk->opt)];//与原来状态量中单差模糊度的偏差的总和j++;}}/* correct phase-bias offset to enssure phase-code coherency 校正相位偏置以确保相位码一致性 */if (j>0) {for (i=1;i<=MAXSAT;i++) {if (rtk->x[IB(i,f,&rtk->opt)]!=0.0) rtk->x[IB(i,f,&rtk->opt)]+=offset/j;//每一项都加上平均偏差，得单差模糊度状态更新}}/* set initial states of phase-bias 利用4求得的bias[i] - rtk->com_bias，来对没有进行初始化的卫星，进行单差相位偏移状态量的初始化。*/for (i=0;i<ns;i++) {if (bias[i]==0.0||rtk->x[IB(sat[i],f,&rtk->opt)]!=0.0) continue;initx(rtk,bias[i],SQR(rtk->opt.std[0]),IB(sat[i],f,&rtk->opt));}free(bias);}
}

周跳检测

/* detect cycle slip by L1-L2 geometry free phase jump -----------------------*/
static void detslp_gf_L1L2(rtk_t *rtk, const obsd_t *obs, int i, int j,const nav_t *nav)
{int sat=obs[i].sat;double g0,g1;trace(3,"detslp_gf_L1L2: i=%d j=%d\n",i,j);if (rtk->opt.nf<=1||(g1=gfobs_L1L2(obs,i,j,nav->lam[sat-1]))==0.0) return;g0=rtk->ssat[sat-1].gf; rtk->ssat[sat-1].gf=g1;if (g0!=0.0&&fabs(g1-g0)>rtk->opt.thresslip) {rtk->ssat[sat-1].slip[0]|=1;rtk->ssat[sat-1].slip[1]|=1;errmsg(rtk,"slip detected GF2_jump (sat=%2d GF_L1_L2=%.3f %.3f)\n",sat,g0,g1);}
}

单差模糊度更新

/* estimate approximate phase-bias by phase - code 对共视星进行循环，利用“单差伪距”和“单差载波相位”计算一个“单差相位偏移”估计值，来对单差相位偏移状态量进行更新。由于如果忽略伪距误差，那么伪距减去载波相位，则应该是载波相位（m）的相位偏移(m)，所以这里计算的是一个大致的相位偏移bias[i]。如果配置为无电离层组合，计算则按照无电离层组合的方式来计算。最后，仅计算所有有效星（单差相位偏移状态不为0）的offset = sum of (bias - phase-bias)。其实就是计算每颗有效星bias[i]与单单差相位偏移状态量的偏差，然后把所有有效星的这个偏差值加起来，之后会除以有效星的个数，最终就是求一个偏差平均值rtk->com_bias。*/for (i=j=0,offset=0.0;i<ns;i++) {if (rtk->opt.ionoopt!=IONOOPT_IFLC) {cp=sdobs(obs,iu[i],ir[i],f); /* cycle 载波相位单差观测值                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              */pr=sdobs(obs,iu[i],ir[i],f+NFREQ);/* 伪距单差观测值 */lami=nav->lam[sat[i]-1][f];if (cp==0.0||pr==0.0||lami<=0.0) continue;bias[i]=cp-pr/lami;//大致的相位偏移}else {cp1=sdobs(obs,iu[i],ir[i],0);cp2=sdobs(obs,iu[i],ir[i],1);pr1=sdobs(obs,iu[i],ir[i],NFREQ);pr2=sdobs(obs,iu[i],ir[i],NFREQ+1);lam1=nav->lam[sat[i]-1][0];lam2=nav->lam[sat[i]-1][1];if (cp1==0.0||cp2==0.0||pr1==0.0||pr2==0.0||lam1<=0.0||lam2<=0.0) continue;C1= SQR(lam2)/(SQR(lam2)-SQR(lam1));C2=-SQR(lam1)/(SQR(lam2)-SQR(lam1));bias[i]=(C1*lam1*cp1+C2*lam2*cp2)-(C1*pr1+C2*pr2);}if (rtk->x[IB(sat[i],f,&rtk->opt)]!=0.0) {offset+=bias[i]-rtk->x[IB(sat[i],f,&rtk->opt)];//与原来状态量中单差模糊度的偏差的总和j++;}}/* correct phase-bias offset to enssure phase-code coherency 校正相位偏置以确保相位码一致性 */if (j>0) {for (i=1;i<=MAXSAT;i++) {if (rtk->x[IB(i,f,&rtk->opt)]!=0.0) rtk->x[IB(i,f,&rtk->opt)]+=offset/j;//每一项都加上平均偏差，得单差模糊度状态更新}}/* set initial states of phase-bias 利用4求得的bias[i] - rtk->com_bias，来对没有进行初始化的卫星，进行单差相位偏移状态量的初始化。*/for (i=0;i<ns;i++) {if (bias[i]==0.0||rtk->x[IB(sat[i],f,&rtk->opt)]!=0.0) continue;initx(rtk,bias[i],SQR(rtk->opt.std[0]),IB(sat[i],f,&rtk->opt));}

3、双差残差及其偏导

此处做了非差残差站间单差和星间双差：
非差残差 = 观测 - 卫地距(此处卫地距包含误差，即：卫地距 = 真实卫地距 + 单位视线向量e * delta_r ) ，然后站间单差星间双差，完成H阵前三列与状态x前三项的乘法计算。后续继续减去双差模糊度，就完成了v = Hx - Z的计算，即新息。（注意ddres完成了v = Z - Hx的计算，后面量测更新中就没有Hx的计算了）

偏导H阵及R阵：

/* double-differenced phase/code residuals -----------------------------------*/
/*O rtk->ssat[i].resp[j] = residual pseudorange error 伪距残差误差O rtk->ssat[i].resc[j] = residual carrier phase error 载波相位残差误差I rtk->rb = base location 基站坐标I nav = sat nav data 导航电文I dt = time diff between base and rover observations(usually 0) 基站流动站观测值间的时间间隔I x = rover pos & vel and sat phase biases(float solution) 流动站位置速度以及卫星相位偏移（浮点解）I P = error covariance matrix of float states 浮点状态误差协方差I sat = list of common sats 共视星列表I y = zero diff residuals(code and phase, base and rover) 无差残差（伪距相位，基站流动站）I e = line of sight unit vectors to sats 观测卫星的观测向量I azel = [az, el] to sats 卫星的方位角和仰角I iu, ir = user and ref indices to sats 用户和基准站观测中对共视卫星的索引I ns = # of sats 卫星数O v = double diff innovations(measurement - model) (phase and code) 双差残差更新（测量 - 模型）（载波相位与伪距）O H = linearized translation from innovations to states(az / el to sats) 量测矩阵HO R = measurement error covariances 量测误差协方差O vflg = bit encoded list of sats used for each double diff 用于每个双差的位编码的sats列表计算双差残差和量测矩阵H
*/
static int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,const double *P, const int *sat, double *y, double *e,double *azel, const int *iu, const int *ir, int ns, double *v,double *H, double *R, int *vflg)
{prcopt_t *opt=&rtk->opt;double bl,dr[3],posu[3],posr[3],didxi=0.0,didxj=0.0,*im;double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,lami,lamj,fi,fj,df,*Hi=NULL;int i,j,k,m,f,ff,nv=0,nb[NFREQ*4*2+2]={0},b=0,sysi,sysj,nf=NF(opt);trace(3,"ddres   : dt=%.1f nx=%d ns=%d\n",dt,rtk->nx,ns);bl=baseline(x,rtk->rb,dr);//b1基线长度，dr基线向量ecef2pos(x,posu); ecef2pos(rtk->rb,posr);//将基站和移动站位置分别从ECEF转到WGS84坐标系（经纬高）// 中间变量初始化Ri=mat(ns*nf*2+2,1); Rj=mat(ns*nf*2+2,1); im=mat(ns,1);tropu=mat(ns,1); tropr=mat(ns,1); dtdxu=mat(ns,3); dtdxr=mat(ns,3);for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++) {rtk->ssat[i].resp[j]=rtk->ssat[i].resc[j]=0.0; //双差伪距残差和双差载波相位残差初始化为0}/* compute factors of ionospheric and tropospheric delay 计算电离层和对流层延迟因子 */for (i=0;i<ns;i++) {if (opt->ionoopt>=IONOOPT_EST) { //如果卡尔曼滤波器包含电离层状态量，调用ionmapf函数分别计算基站和移动站处的投影函数，然后求其平均值作为“单差垂直电离层延迟状态量”的投影系数im[i]=(ionmapf(posu,azel+iu[i]*2)+ionmapf(posr,azel+ir[i]*2))/2.0;}if (opt->tropopt>=TROPOPT_EST) { //如果卡尔曼滤波器中包含对流层状态量，调用prectrop函数计算基站和移动站的对流层延迟湿分量，存在tropu[i]和tropur[i]中tropu[i]=prectrop(rtk->sol.time,posu,0,azel+iu[i]*2,opt,x,dtdxu+i*3);tropr[i]=prectrop(rtk->sol.time,posr,1,azel+ir[i]*2,opt,x,dtdxr+i*3);}}for (m=0;m<5;m++) /* m=0:gps/sbs,1:glo,2:gal,3:bds,4:qzs *///对各个系统以及载波相位、伪距分别进行循环处理for (f=opt->mode>PMODE_DGPS?0:nf;f<nf*2;f++) {/* search reference satellite with highest elevation 找到高度角最大的卫星作为参考星 */for (i=-1,j=0;j<ns;j++) {sysi=rtk->ssat[sat[j]-1].sys;if (!test_sys(sysi,m)) continue;if (!validobs(iu[j],ir[j],f,nf,y)) continue;if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) i=j;}if (i<0) continue;/* make double difference 利用zdres函数中计算的载波相位/伪距无差残差，计算每颗卫星的载波相位/伪距双差残差，并对观测矩阵H中和位置状态量相关的部分进行赋值；*/for (j=0;j<ns;j++) {if (i==j) continue;sysi=rtk->ssat[sat[i]-1].sys;sysj=rtk->ssat[sat[j]-1].sys;if (!test_sys(sysj,m)) continue;if (!validobs(iu[j],ir[j],f,nf,y)) continue;ff=f%nf;lami=nav->lam[sat[i]-1][ff];lamj=nav->lam[sat[j]-1][ff];if (lami<=0.0||lamj<=0.0) continue;if (H) {Hi=H+nv*rtk->nx; //将H+nv*rtk->nx的地址赋给H，此时更改Hi中的值即更改H矩阵中的值for (k=0;k<rtk->nx;k++) Hi[k]=0.0;}/* double-differenced residual 双差残差此处做了非差残差站间单差和星间双差，非差残差 = 观测 - 卫地距(此处卫地距包含误差，即：卫地距 = 真实卫地距 + 单位视线向量e *                 delta_r ) ，然后站间单差星间双差，完成H阵前三列与状态x前三项的乘法计算。后续代码继续减去双差模糊度，就完成了v = Hx - Z的计算，即新息。 */v[nv]=(y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])-(y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2]);/* partial derivatives by rover position 移动站位置偏导 */if (H) {for (k=0;k<3;k++) {Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3];//每一行前3列为观测向量差}}/* double-differenced ionospheric delay term 如果卡尔曼滤波中包含电离层状态量，以GPS L1为基准（lam_carr[0]为L1的波长，该全局变量的初始化在rtkcom.c中），计算当前频率的电离层单差延迟量，需要注意伪距和载波相位的电离层延迟大小相同，但是符号相反。然后，从2）计算的载波相位/伪距双差残差值中扣除该卫星的双差电离层延迟量，并对观测方程H中和电离层状态量相关的部分进行赋值；*/if (opt->ionoopt==IONOOPT_EST) {fi=lami/lam_carr[0]; fj=lamj/lam_carr[0];didxi=(f<nf?-1.0:1.0)*fi*fi*im[i];didxj=(f<nf?-1.0:1.0)*fj*fj*im[j];v[nv]-=didxi*x[II(sat[i],opt)]-didxj*x[II(sat[j],opt)];if (H) {Hi[II(sat[i],opt)]= didxi;Hi[II(sat[j],opt)]=-didxj;}}/* double-differenced tropospheric delay term 如果卡尔曼滤波中包含对流层状态量，从载波相位/伪距双差残差值中扣除对流层双差湿分量，并对观测方程H中和对流层状态量相关的部分进行赋值*/if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) {v[nv]-=(tropu[i]-tropu[j])-(tropr[i]-tropr[j]);for (k=0;k<(opt->tropopt<TROPOPT_ESTG?1:3);k++) {if (!H) continue;Hi[IT(0,opt)+k]= (dtdxu[k+i*3]-dtdxu[k+j*3]);Hi[IT(1,opt)+k]=-(dtdxr[k+i*3]-dtdxr[k+j*3]);}}/* double-differenced phase-bias term 如果不是无电离层组合，从载波相位双差残差中扣除双差模糊度部分（即phase-bias），并对H矩阵中和模糊度相关的部分进行赋值*/if (f<nf) {if (opt->ionoopt!=IONOOPT_IFLC) {v[nv]-=lami*x[IB(sat[i],f,opt)]-lamj*x[IB(sat[j],f,opt)];if (H) {Hi[IB(sat[i],f,opt)]= lami;Hi[IB(sat[j],f,opt)]=-lamj;}}else {v[nv]-=x[IB(sat[i],f,opt)]-x[IB(sat[j],f,opt)];if (H) {Hi[IB(sat[i],f,opt)]= 1.0;Hi[IB(sat[j],f,opt)]=-1.0;}}}/* glonass receiver h/w bias term */if (rtk->opt.glomodear==2&&sysi==SYS_GLO&&sysj==SYS_GLO&&ff<NFREQGLO) {df=(CLIGHT/lami-CLIGHT/lamj)/1E6; /* freq-difference (MHz) */v[nv]-=df*x[IL(ff,opt)];if (H) Hi[IL(ff,opt)]=df;}/* glonass interchannel bias correction */else if (sysi==SYS_GLO&&sysj==SYS_GLO) {v[nv]-=gloicbcorr(sat[i],sat[j],&rtk->opt,lami,lamj,f);}//最后将载波相位/伪距残差存入对应变量中。if (f<nf) rtk->ssat[sat[j]-1].resc[f   ]=v[nv];else      rtk->ssat[sat[j]-1].resp[f-nf]=v[nv];/* test innovation 对载波相位/伪距双差残差进行检查，如果过大超过阈值，则认为无效 */if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {if (f<nf) {rtk->ssat[sat[i]-1].rejc[f]++;rtk->ssat[sat[j]-1].rejc[f]++;}errmsg(rtk,"outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n",sat[i],sat[j],f<nf?"L":"P",f%nf+1,v[nv]);continue;}/* single-differenced measurement error variances 调用varerr函数计算载波相位/伪距单差的量测噪声方差*/Ri[nv]=varerr(sat[i],sysi,azel[1+iu[i]*2],bl,dt,f,opt);Rj[nv]=varerr(sat[j],sysj,azel[1+iu[j]*2],bl,dt,f,opt);/* set valid data flags 设置卫星有效标志 */if (opt->mode>PMODE_DGPS) {if (f<nf) rtk->ssat[sat[i]-1].vsat[f]=rtk->ssat[sat[j]-1].vsat[f]=1;}else {rtk->ssat[sat[i]-1].vsat[f-nf]=rtk->ssat[sat[j]-1].vsat[f-nf]=1;}trace(4,"sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n",sat[i],sat[j],f<nf?"L":"P",f%nf+1,v[nv],Ri[nv],Rj[nv]);vflg[nv++]=(sat[i]<<16)|(sat[j]<<8)|((f<nf?0:1)<<4)|(f%nf);nb[b]++;}
#if 0 /* residuals referenced to reference satellite (2.4.2 p11) *//* restore single-differenced residuals assuming sum equal zero */if (f<nf) {for (j=0,s=0.0;j<MAXSAT;j++) s+=rtk->ssat[j].resc[f];s/=nb[b]+1;for (j=0;j<MAXSAT;j++) {if (j==sat[i]-1||rtk->ssat[j].resc[f]!=0.0) rtk->ssat[j].resc[f]-=s;}}else {for (j=0,s=0.0;j<MAXSAT;j++) s+=rtk->ssat[j].resp[f-nf];s/=nb[b]+1;for (j=0;j<MAXSAT;j++) {if (j==sat[i]-1||rtk->ssat[j].resp[f-nf]!=0.0)rtk->ssat[j].resp[f-nf]-=s;}}
#endifb++;}/* end of system loop *//* baseline length constraint for moving baseline 如果是动基线的模式，增加基线长度约束 */if (opt->mode==PMODE_MOVEB&&constbl(rtk,x,P,v,H,Ri,Rj,nv)) {vflg[nv++]=3<<4;nb[b++]++;}if (H) {trace(5,"H=\n"); tracemat(5,H,rtk->nx,nv,7,4);}/* double-differenced measurement error covariance 调用ddcov计算载波相位/伪距双差量测噪声协方差R阵 */ddcov(nb,b,Ri,Rj,nv,R);free(Ri); free(Rj); free(im);free(tropu); free(tropr); free(dtdxu); free(dtdxr);return nv;
}

4、卡尔曼滤波量测更新

/* 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]
*-----------------------------------------------------------------------------*/
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;
}

5、验证解的有效性

利用卡尔曼滤波后得到的位置解计算后验双差残差，将此残差与阈值进行比较，若大于阈值则说明此卫星的残差过大，有问题。然而RTKLIB代码中只检测出残差过大的卫星，并没有对卫星做任何后续处理（处理的部分程序用了卡方检测，被注释掉了）。

/* validation of solution ----------------------------------------------------*/
static int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg,int nv, double thres)
{#if 0prcopt_t *opt=&rtk->opt;double vv=0.0;
#endifdouble fact=thres*thres;int i,stat=1,sat1,sat2,type,freq;char *stype;trace(3,"valpos  : nv=%d thres=%.1f\n",nv,thres);/* post-fit residual test */for (i=0;i<nv;i++) {if (v[i]*v[i]<=fact*R[i+i*nv]) continue;sat1=(vflg[i]>>16)&0xFF;sat2=(vflg[i]>> 8)&0xFF;type=(vflg[i]>> 4)&0xF;freq=vflg[i]&0xF;stype=type==0?"L":(type==1?"L":"C");errmsg(rtk,"large residual (sat=%2d-%2d %s%d v=%6.3f sig=%.3f)\n",sat1,sat2,stype,freq+1,v[i],SQRT(R[i+i*nv]));}
#if 0 /* omitted v.2.4.0 */if (stat&&nv>NP(opt)) {/* chi-square validation */for (i=0;i<nv;i++) vv+=v[i]*v[i]/R[i+i*nv];if (vv>chisqr[nv-NP(opt)-1]) {errmsg(rtk,"residuals validation failed (nv=%d np=%d vv=%.2f cs=%.2f)\n",nv,NP(opt),vv,chisqr[nv-NP(opt)-1]);stat=0;}else {trace(3,"valpos : validation ok (%s nv=%d np=%d vv=%.2f cs=%.2f)\n",time_str(rtk->sol.time,2),nv,NP(opt),vv,chisqr[nv-NP(opt)-1]);}}
#endifreturn stat;
}

6、固定整周模糊度

实际上，还有一种最简单的，直接就近取整固定。模糊度固定要做的事情，就是将一组各项有不同误差的小数变为整数，如果不考虑各项的误差，就是就近取整固定法，比如将[3.9, 5.1, 9.8]变为[4, 5, 10]。模糊度固定是一个数学问题，输入为一组浮点数及其协方差矩阵，输出一组整数及其协方差矩阵，初学可以将其黑箱处理。
RTKLIB中用的LAMBDA方法：

/* resolve integer ambiguity by LAMBDA ---------------------------------------*/
/*通过LAMBDA算法求解整周模糊度IO  rtk_t *rtk:      rtk solution structureI   double *bias     利用lambda算法计算得到的双差整周模糊度IO  double *xa       fixed states（在注意事项中进行了更为详细的解释）
*/
static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa)
{prcopt_t *opt=&rtk->opt;int i,j,ny,nb,info,nx=rtk->nx,na=rtk->na;double *D,*DP,*y,*Qy,*b,*db,*Qb,*Qab,*QQ,s[2];trace(3,"resamb_LAMBDA : nx=%d\n",nx);rtk->sol.ratio=0.0;/*首先检查配置中所设置的AR阈值（LAMBDA算法最优解和次优解的比值，通常取3.0），如果该阈值小于1，则返回0。检查卡尔曼滤波中位置状态量的协方差阵，？？？如果位置方差超过所设定的方差阈值，则返回0，避免由于方差过大造成false fix。*/if (rtk->opt.mode<=PMODE_DGPS||rtk->opt.modear==ARMODE_OFF||rtk->opt.thresar[0]<1.0) {return 0;}/* single to double-difference transformation matrix (D') 调用ddmat函数，创建将卡尔曼状态量从单差转到双差的转换矩阵D’。主要是将单差相位偏移状态量转换为双差相位偏移，这里的D’阵实际就是RTKlib manual 165页中的G阵。*/D=zeros(nx,nx);if ((nb=ddmat(rtk,D))<=0) {errmsg(rtk,"no valid double-difference\n");free(D);return 0;}/*定义几个变量和矩阵，其中ny=na+nb，na实际就是之前卡尔曼滤波中除了单差相位偏移之外的所有状态量个数（例如：位置+速度+加速度+电离层+对流层……），nb则是双差相位偏移的个数（即需要解算的整周模糊度个数）；*/ny=na+nb; y=mat(ny,1); Qy=mat(ny,ny); DP=mat(ny,nx);b=mat(nb,2); db=mat(nb,1); Qb=mat(nb,nb); Qab=mat(na,nb); QQ=mat(na,nb);/* transform single to double-differenced phase-bias (y=D'*x, Qy=D'*P*D) 根据RTKlib manual 165页公式（E.7.15）、（E.7.16）计算双差状态量和双差协方差阵。之所以要在做LAMBDA算法前将单差转成双差，是为了去除接收机的初始相位偏移，这样就只剩下整周的模糊度。*/matmul("TN",ny, 1,nx,1.0,D ,rtk->x,0.0,y );matmul("TN",ny,nx,nx,1.0,D ,rtk->P,0.0,DP);matmul("NN",ny,ny,nx,1.0,DP,D     ,0.0,Qy);/* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) 计算公式（E.7.16）中的Q N，Q_NR矩阵。*/for (i=0;i<nb;i++) for (j=0;j<nb;j++) Qb [i+j*nb]=Qy[na+i+(na+j)*ny];for (i=0;i<na;i++) for (j=0;j<nb;j++) Qab[i+j*na]=Qy[   i+(na+j)*ny];trace(4,"N(0)="); tracemat(4,y+na,1,nb,10,3);/* lambda/mlambda integer least-square estimation 调用lambda函数计算双差整周模糊度最优解以及残差。*/if (!(info=lambda(nb,2,y+na,Qb,b,s))) {trace(4,"N(1)="); tracemat(4,b   ,1,nb,10,3);trace(4,"N(2)="); tracemat(4,b+nb,1,nb,10,3);rtk->sol.ratio=s[0]>0?(float)(s[1]/s[0]):0.0f;if (rtk->sol.ratio>999.9) rtk->sol.ratio=999.9f;/* validation by popular ratio-test *//*如果最优解和次优解的比值大于阈值，则利用公式（E.7.19），根据lambda算法得到的双差整周模糊度，计算除了单差相位偏移之外的所有状态量（例如：位置+速度+加速度+电离层+对流层……），存入rtk->xa中。这一步实际就是利用lambda算法得到的整数的整周模糊度对其他状态量进行修正。*/if (s[0]<=0.0||s[1]/s[0]>=opt->thresar[0]) {/* transform float to fixed solution (xa=xa-Qab*Qb\(b0-b)) */for (i=0;i<na;i++) {rtk->xa[i]=rtk->x[i];for (j=0;j<na;j++) rtk->Pa[i+j*na]=rtk->P[i+j*nx];}for (i=0;i<nb;i++) {bias[i]=b[i];y[na+i]-=b[i];}if (!matinv(Qb,nb)) {matmul("NN",nb,1,nb, 1.0,Qb ,y+na,0.0,db);matmul("NN",na,1,nb,-1.0,Qab,db  ,1.0,rtk->xa);/* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */matmul("NN",na,nb,nb, 1.0,Qab,Qb ,0.0,QQ);matmul("NT",na,na,nb,-1.0,QQ ,Qab,1.0,rtk->Pa);trace(3,"resamb : validation ok (nb=%d ratio=%.2f s=%.2f/%.2f)\n",nb,s[0]==0.0?0.0:s[1]/s[0],s[0],s[1]);/* restore single-differenced ambiguity 调用restamb函数，利用lambda算法计算得到的双差整周模糊度，重新计算单差相位偏移，并存入xa中，同时将第8步中得到的其他状态量也存入xa中。*/restamb(rtk,bias,nb,xa);}else nb=0;}else { /* validation failed */errmsg(rtk,"ambiguity validation failed (nb=%d ratio=%.2f s=%.2f/%.2f)\n",nb,s[1]/s[0],s[0],s[1]);nb=0;}}else {errmsg(rtk,"lambda error (info=%d)\n",info);nb=0;}free(D); free(y); free(Qy); free(DP);free(b); free(db); free(Qb); free(Qab); free(QQ);return nb; /* number of ambiguities */
}