rtk_ins紧组合（iGnav）

前言：

本文是基于IGNAV的RTK/INS紧组合学习，IGNAV是基于RTKLIB进行二次开发的，因此熟悉RTKLIB源代码的学习起来会容易一些。在学习一个项目之前，对算法的学习至关重要，对算法的理解程度很大程度上决定了你对代码的理解能力，因此IGNAV项目中man文件夹内的theory.pdf。算法文件中6.3.2-GNSS 伪距与载波观测信息辅助INS的过程推到，务必花时间理解。

一、IGNAV的RTK/INS流程

下图是IGNAV中RTK/INS紧组合流程。其中rtksvrthread是定位计算线程，tcigpos是紧组合入口函数，也是本文主要学习对象。对文件读取，输出的内容不做介绍!

注意图中函数所在文件行数仅供参考！！！

updatetimediff()

 /* ins tightly coupled position mode */
    if (svr->rtk.opt.mode==PMODE_INS_TGNSS) {
        if (syn->ni&&syn->nr) {//IMU和流动站时间对齐索引均不为0
            syn->dt[0]=time2gpst(syn->time[2],NULL)-//IMU时间-流动站观测值时间
                       time2gpst(syn->time[0],NULL);
        }
        if (syn->ni&&syn->nb) {//基站模式
            syn->dt[1]=time2gpst(syn->time[2],NULL)-
                       time2gpst(syn->time[1],NULL);
        }
        if (svr->rtk.opt.insopt.pose_aid||svr->rtk.opt.insopt.align_dualants) {//双天线模式
            if (syn->np&&syn->ni) {
                syn->dt[2]=time2gpst(syn->time[5],NULL)-
                           time2gpst(syn->time[2],NULL);
            }
        }
    }

在RTRK/INS紧组合中，主要更新**dt[0]：**当前历元的IMU时间-流动站时间

timealign()

此处进入紧组合时间对齐

if (svr->rtk.opt.mode==PMODE_INS_TGNSS) return imuobsalign(svr);

imuobsalign()

    int i,j,k,n; double sow1,sow2,sow3;
    obs_t *p1=NULL,*p2=NULL;
    syn_t *psyn=&svr->syn;

    /* observation and imu data time alignment */
    n=psyn->of[2]?MAXIMUBUF:psyn->ni;//n为IMU观测历元数
    p1=svr->obs[0];//流动站观测值
    p2=svr->obs[1];//基站观测值

在navlib.h中查看syn_t结构体(存储了各类数据的索引)：

typedef struct {         /* time synchronization index in buffer */
    int ni,nip,imu;      /* current/precious number and sync index of imu measurement data  */
    int nr,nrp,rover;    /* current/precious number and sync index of observation data */
    int ns,nsp,pvt;      /* current/precious number and sync index of pvt solution data */
    int nb,nbp,base;     /* current/precious number and sync index of base observation data */
    int nm,nmp,img;      /* current/precious number and sync index of image raw data */
    int np,npp,pose;     /* current/precious number and sync index of pose measurement */
    int of[6];           /* overflow flag (rover,base,imu,pvt,image,pose) */
    unsigned int tali[6];/* time alignment for data (rover-base,pvt-imu,rover-base-imu,imu-image,pose-imu) */
    unsigned int ws;     /* search window size */
    double dt[6];        /* time difference between input streams */
    gtime_t time[6];     /* current time of rover,base,imu,pvt,image and pose measurement */
} syn_t;

在imuobsalign()中时间对齐只能匹配一个IMU和GNSS观测值，且对齐方式与KF_GINS的区别还是挺大的。

/* observation and imu data time alignment */
    n=psyn->of[2]?MAXIMUBUF:psyn->ni;//n为IMU观测历元数
    p1=svr->obs[0];//流动站观测值窗口
    p2=svr->obs[1];//基站观测值窗口

    for (i=0;i<n&&svr->syn.tali[2]!=2;i++) { /* start time alignment 从窗口内第一个IMU观测值开始*/

        sow1=time2gpst(svr->imu[i].time,NULL);//将imu时间转换为周内秒

        /* match rover observation匹配流动站观测值 */
        for (j=0;j<(psyn->of[0]?MAXOBSBUF:psyn->nr);j++) {//循环遍历流动站每一个历元观测值
            sow2=time2gpst(p1[j].data[0].time,NULL);//将流动站观测值时间转换为周内秒
            if (p1[j].n) {//流动站观测值存在
                if (fabs(sow1-sow2)>DTTOL) continue;//流动站观测值时间不在IMU时间阈值0.0025s内
            }
            psyn->imu    =i;//记录第i个IMU匹配到相应的流动站观测值
            psyn->rover  =j;//记录第j个流动站观测值匹配到相应的IMU观测值
            psyn->tali[2]=1;//完成标识
            break;
        }
        /* match base observation 基站同上*/
        if (psyn->tali[2]==1) {
            for (k=0;k<(psyn->of[1]?MAXOBSBUF:psyn->nb);k++) {
                sow3=time2gpst(p2[k].data[0].time,NULL);
                if (p2[k].n) {
                    if (fabs(sow2-sow3)>DTTOLM) continue;
                }
                else continue;
                psyn->base   =k;
                psyn->tali[2]=2;
                break;
            }
        }
        if (psyn->tali[2]==2) {//匹配完一个GNSS观测值就退出
            tracet(3,"imu and rover/base align ok\n");
            return 1;
        }
        else psyn->tali[2]=0; /* fail */

inputobstc()

 if ((obsd.n=inputobstc(svr,imus.data[i].time,obsd.data))) {
                j=INSUPD_MEAS;
            }

**rtksvr_t *srv:**RTK server type囊括所有原始数据，中间数据，结果数据，参数设置等

**imus.data[i].time:**imu数据窗口第i个数据的时间

**obsd.data:**GNSS观测值窗口内某个历元的观测数据，为空

    const obs_t *robs=svr->obs[0],*bobs=svr->obs[1];//流动站与基站观测数据窗口

    /* match rover observation data 匹配流动站观测值数据*/
    for (i=0,dt=0.0,n=NS(svr->syn.rover,svr->syn.nr,MAXOBSBUF);
         i<n+1&&svr->syn.nr;i++) {
        j=svr->syn.rover+i;//timealign()匹配的观测值所在历元索引+i

        if (j>=MAXOBSBUF) j=j%MAXOBSBUF;//超出历元窗口时，余上窗口大小
        if (dt&&fabs(dt)<fabs(timediff(time,robs[j].data[0].time))) {
            break;//第j个历元的时间与目标IMU测量时间大于大于指定阈值
        }
        if (fabs((dt=timediff(time,robs[j].data[0].time)))<DTTOL//当前观测值历元时间与指定IMU时间小于阈值时
            &&robs[j].n!=0) {//该GNSS历元内观测值数不为0
            for (k=0,m=0;k<robs[j].n;k++) obs[m++]=robs[j].data[k];//将时间对齐的数据输入到obs[]
            svr->syn.rover=j; tr=obs[0].time;//记录流动站观测值历元索引和历元时间
            flag=1; break;//流动站配置完毕
        }
    }

inputimu()

/* input imu measurement data输入IMU数据并确保正确的时间对齐。-----------*/
static int inputimu(rtksvr_t *svr,imud_t *data)
{
    int i,j,k,n=0;

    tracet(3,"inputimu:\n");

    /* check time alignment of input streams */
    if (!svr->syn.tali[1]&&!svr->syn.tali[2]&&!svr->syn.tali[3]) {
        trace(2,"check time alignment fail\n");
        return 0;
    }
    for (i=0,k=0,n=NS(svr->syn.imu,svr->syn.ni,MAXIMUBUF),
        j=svr->syn.imu;i<n+1;i++) {//j为IMU时间对齐索引
        j=svr->syn.imu+i;//从对齐索引的数据开始
        if (j>=MAXIMUBUF) j=j%MAXIMUBUF;//超出MAXIMUBUF时，j取模循环
        if (k<MAXIMU) data[k++]=svr->imu[j]; else break;//提取数据
    }
    svr->syn.imu=(j+1)%MAXIMUBUF;//重置时间对齐索引svr->syn.imu
    return k;//返回k的值，表示处理的IMU数据历元数。
}

其中

n=((((svr->syn.ni)-1)%(1000)-(svr->syn.imu))<0?(((svr->syn.ni)-1)%(1000)-(svr->syn.imu)+(1000)):(((svr->syn.ni)-1)%(1000)-(svr->syn.imu)))

计算了IMU当前历元编号和IMU同步索引之间的历元数。如果结果为负数，它会加上1000，以确保得到正确的差值。

insinirtobs()

 double vr[3]={0}; insstate_t *ins=&svr->rtk.ins;// 初始化速度为零

    /* global variables for rtk positioning */
    static int first=1,i;
    static prcopt_t popt=svr->rtk.opt;
    static rtk_t rtk={0};// 初始化 RTK 结构体
    static sol_t sols[MAXSOL]={0};// 保存位置解的数组

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

    svr->rtk.ins.stat=INSS_INIT;
    if (n<=0) {
        trace(2,"no observation data to initial\n"); return 0;
    }
    /* initial gps position options */
    if (first) {//如果是首次调用，初始化RTK定位选项
        initrtkpos(&rtk,&popt); first=0;
    }
    rtkpos(&rtk,obs,n,&svr->nav);// GNSS计算定位结果

    /* save position solution to buffer将最新的位置解保存到缓冲区 */
    for (i=0;i<MAXSOL-1;i++) sols[i]=sols[i+1]; sols[i]=rtk.sol;
    for (i=0;i<MAXSOL;i++) {//检查解的状态，确保解是有效的
        if (sols[i].stat>popt.insopt.iisu||sols[i].stat==SOLQ_NONE) {
            trace(2,"check solution status fail\n");
            return 0;
        }
    }//检查解的时间差异，确保时间差异在 MAXDIFF=10s 内
    for (i=0;i<MAXSOL-1;i++) {
        if (timediff(sols[i+1].time,sols[i].time)>MAXDIFF) {
            return 0;
        }
    }
    /* compute velocity from solutions 从解中计算速度*/
    matcpy(vr,sols[MAXSOL-1].rr+3,1,3);
    if (norm(vr,3)==0.0) {
        sol2vel(sols+MAXSOL-1,sols+MAXSOL-2,vr);
    }//检查陀螺仪测量值和速度的范数是否在合理的范围内
    if (norm(imu->gyro,3)>MAXGYRO||norm(vr,3)<MINVEL) {
        return 0;
    }
    /* initialize ins states初始化 INS 状态 */
    initinsrt(svr);
    if (!ant2inins(sols[MAXSOL-1].time,sols[MAXSOL-1].rr,vr,&popt.insopt,
                   NULL,ins,NULL)) {//将位置解和速度传递给INS进行初始化
        return 0;
    }
    ins->time=sols[MAXSOL-1].time;

    /* update ins state in n-frame n系(导航系下)更新INS状态 */
    update_ins_state_n(ins);

二、tcigpos()

    trace(3,"tcigpos: update=%d,time=%s\n",upd,time_str(imu->time,4));

#if CHKNUMERIC
    /* check numeric of estimate state检查状态估计数字 */
    for (i=0;i<3;i++) {
        if (isnan(ins->re[i])||isnan(ins->ve[i])||isnan(ins->ae[i])||
            isinf(ins->re[i])||isinf(ins->ve[i])||isinf(ins->ae[i])) {
            fprintf(stderr,"check numeric error: nan or inf\n");
            return 0;
        }
    }
#endif
    ins->stat=INSS_NONE; /* start ins mechanization开始机械编排 */
    if (
#if 0
        /* update ins states based on llh position mechanization */
        !updateinsn(insopt,ins,imu);
#else
        /* update ins states in e-frame */
        !updateins(insopt,ins,imu)
        ) {
#endif
        trace(2,"ins mechanization update fail\n");
        return 0;
    }
    P=zeros(nx,nx);

    /* propagate ins states 传播INS状态到GNSS历元时刻*/
    propinss(ins,insopt,ins->dt,ins->x,ins->P);

    /* check variance of estimated position检查位置方差 */
    chkpcov(nx,insopt,ins->P);

    /* updates ins states using imu or observation data开始更新INS状态 */
    if (upd==INSUPD_TIME) {
        ins->stat=INSS_TIME;
        info=1;
    }
    else {
        for (i=0;i<6;i++) rtk->sol.pqr[i]=rtk->sol.qr[i];
        rtk->sol.pstat=rtk->sol.stat;
        ins->gstat=SOLQ_NONE;
        ins->ns=0;
#if REBOOT
        /* reboot tightly coupled if need 重启紧组合*/
        if ((flag=rebootc(ins,opt,obs,n,imu,nav))) {
            if (flag==1) {
                trace(2,"ins tightly coupled still reboot\n");
                info=0; goto EXIT;
            }
            trace(3,"ins tightly coupled reboot ok\n");
            ins->stat=INSS_REBOOT; info=1;
            goto EXIT;
        }
#endif
        /* updates by measurement data通过测量数据更新 */
        if (obs&&imu&&n) {

            /* count rover/base station observations 记录流动站/基站观测数*/
            for (nu=0;nu   time);

            /* check synchronization 检查同步*/
            if (fabs(dt)>3.0) {
                trace(2,"observation and imu sync error\n");
                info=0;
            }
            /* tightly coupled进入紧组合 */
            if (info) {
                info=rtkpos(rtk,obs,nu+nr,nav);
            }
        }
        else info=0;

        /* update coupled solution status 更新组合状态解/反馈*/
        if (info) {
            ins->ptct=ins->time;
            ins->stat=ins->stat==INSS_REBOOT?INSS_REBOOT:INSS_TCUD;

            trace(3,"tightly couple ok\n");

            /* lack satellites but tightly-coupled run 缺少卫星的*/
            if (ins->ns<4) {
                ins->stat=INSS_LACK;
            }
            /* save precious epoch gps measurement 保存GPS精密?历元测量值*/
            savegmeas(ins,&rtk->sol,NULL);
#if 1
            /* recheck attitude检查姿态 */
            rechkatt(ins,imu);
#endif
            /* ins state in n-frame 导航坐标系下的ins状态*/
            update_ins_state_n(ins);
        }
        else {
            trace(2,"tightly coupled fail\n");
            info=0;
        }
    }
EXIT:
    free(P); return info;

其中机械编排updateins()，状态传播propinss()，紧组合rtkpos(),函数是重要学习内容。

三、机械编排updateins()

通过结合陀螺仪和加速度计的测量值，更新姿态、速度和位置信息

   /* save precious epoch ins states保存上一个历元的 INS 状态 */
    savepins(ins,data);
    //当前IMU测量时间 data->time与上一个INS状态的时间ins->time之间的时间差
    if ((dt=timediff(data->time,ins->time))>MAXDT||fabs(dt)<1E-6) {

        /* update time information */
        ins->dt=timediff(data->time,ins->time);//时间间隔
        ins->ptime=ins->time;//上一个历元解算时间
        ins->time =data->time;//最新IMU测量时间

        trace(2,"time difference too large: %.0fs\n",dt);
        return 0;
    }

1、IMU测量值校正

    for (i=0;i<3;i++) {
        ins->omgb0[i]=data->gyro[i];
        ins->fb0  [i]=data->accl[i];
        if (insopt->exinserr) {//ins测量值校正。
            ins_errmodel(data->accl,data->gyro,ins->fb,ins->omgb,ins);
        }
        else {//否则，直接更新陀螺仪和加速度计的测量值。
            ins->omgb[i]=data->gyro[i]-ins->bg[i];
            ins->fb  [i]=data->accl[i]-ins->ba[i];
        }
    }

用ins_errmodel()函数对IMU测量值（角速率，比力）进行改正，否则，直接更新陀螺仪和加速度计的测量值。

2、更新姿态

首先用rotscull_corr() 函数进行旋转和划桨运动校正

rvec2quat(domgb,dqb) 旋转向量domgb[]->四元数dqb[]

quat2dcmx(dqe,dCe) 四元数dqe[]->转换矩阵dCe[]

dcm2quatx(ins->Cbe,qk_1)

quatmulx(qk_1,dqb,qtmp) 四元数乘法

normquat(qk) 规范化四元数

quat2dcmx(qk,ins->Cbe) 四元数qk->转换矩阵ins->Cbe

3、更新速度

**gravity(ins->re,ge)：**根据地心地固坐标re[]和重力模型求这点的重力ge[]

 for (i=0;i<3;i++) {
        ins->ve[i]+=dvfk[i]+(ge[i]-2.0*wv[i])*dt;//更新速度
    }

4、更新位置

 /* update position */
    matcpy(vek_1,ins->ve,1,3);
    for (i=0;i<3;i++) {
        ins->re[i]+=0.5*(vek_1[i]+ins->ve[i])*dt;
    }

理论应该是位置增量=（前一历元速度+当前历元速度）*dt *0.5建模成匀加速运动，但是在此处，vek_1[]获取的值还是当前的历元速度，实际上建模成了匀速运动，在短时间内，效果基本一样。

**updinsn(ins)：**更新INS在n下系的坐标，之前的更新都是在e系下的

四、propinss()

进行惯导递推(将INS状态和协方差递推到当前IMU测量时刻)。

updstst()

此函数更新状态和协方差，为后面的扩展卡尔曼滤波做基础

 /* determine approximate system noise covariance matrix确定近似系统噪声协方差矩阵 */
    opt->exprn?getprn(ins,opt,dt,Q):
               getQ(opt,dt,Q);

    /* determine transition matrix确定转移矩阵
     * using last epoch ins states (first-order approx)使用最后历元ins状态（一阶近似值）*/
    opt->exphi?precPhi(opt,dt,ins->Cbe,ins->re,ins->omgb,ins->fb,phi):
               getPhi1(opt,dt,ins->Cbe,ins->re,ins->omgb,ins->fb,phi);

#if UPD_IN_EULER
    getphi_euler(opt,dt,ins->Cbe,ins->re,ins->omgb,ins->fb,phi);
#endif
    /* propagate state estimation error covariance 传播状态估计误差协方差*/
    if (fabs(dt)>=MAXUPDTIMEINT) {//如果时间步长大于等于MAXUPDTIMEINT，
        getP0(opt,P);           //则直接调用函数getP0来获取初始状态估计误差协方差矩阵
    }
    else {//通过转移矩阵和协方差矩阵来预测下一个时间步的状态估计误差协方差矩阵
        propP(opt,Q,phi,P0,P);
    }
    /* propagate state estimates noting that
     * all states are zero due to close-loop correction */
    if (x) propx(opt,x0,x);//预测状态估计值

    /* predict info. */
    if (ins->P0) matcpy(ins->P0,P  ,ins->nx,ins->nx);
    if (ins->F ) matcpy(ins->F ,phi,ins->nx,ins->nx);

之后在chkpcov()检查INS状态协方差矩阵

int i; double var=0.0;
    for (i=xiP(opt);i<xiP(opt)+3;i++) var+=SQRT(P[i+i*nx]);

    if ((var/3)>MAXVAR) if (P) getP0(opt,P);

如果P矩阵位置参数对应的对角阵阵上的均值大于阈值MAXVAR，就初始化P

五、rtkpos()

prcopt_t *opt=&rtk->opt;
    insopt_t *insopt=&opt->insopt;
    sol_t solb={{0}};
    gtime_t time;
    static obsd_t obsd[MAXOBS];
    int fi=0,fj=1,fk=2;
    int i,j,nu,nr,stat=0,tcs=0,tcp=0;
    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);
    trace(5,"nav=\n"); tracenav(5,nav);

    /* check tc-mode */
    tcs=opt->mode==PMODE_INS_TGNSS&&insopt->tc==INSTC_SINGLE;//spp_ins紧组合
    tcp=opt->mode==PMODE_INS_TGNSS&&insopt->tc==INSTC_PPK;//ppp_ins紧组合

    if (n<=0) return 0;

    /* 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;
    }
    /* count rover/base station observations */
    for (nu=0;nu   <n&&obs[nu   ].rcv==1;nu++) ;//流动站观测卫星计数
    for (nr=0;nu+nr<n&&obs[nu+nr].rcv==2;nr++) ;//基站观测卫星计数

    /* for rover and base observation data */
    for (i=0;i<nu+nr&&opt->adjobs;i++) {

        memcpy(&obsd[i],&obs[i],sizeof(obsd_t));
        if (adjsind(opt,&obs[i],&fi,&fj,&fk)) {//调整观测数据
            trace(4,"adjust observation data signal index ok\n");
        }
        /* here just adjust three frequency */
        for (j=0;j<3;j++) {
            obsd[i].LLI[j]=obs[i].LLI[j==0?fi:j==1?fj:fk];
            obsd[i].SNR[j]=obs[i].SNR[j==0?fi:j==1?fj:fk];

            obsd[i].P[j]=obs[i].P[j==0?fi:j==1?fj:fk];
            obsd[i].L[j]=obs[i].L[j==0?fi:j==1?fj:fk];
            obsd[i].D[j]=obs[i].D[j==0?fi:j==1?fj:fk];
        }
        /* index of frequency */
        fi=0; fj=1; fk=2;
    }
    if (opt->adjobs) {
        trace(4,"adjust obs=\n"); traceobs(4,obsd,n);
    }
    /* previous epoch */
    time=rtk->sol.time;
    
    /* rover position by single point positioning 对流动站进行单点定位*/
    if (!pntpos(opt->adjobs?obsd:obs,nu,nav,&rtk->opt,&rtk->sol,tcs?&rtk->ins:NULL,
                NULL,rtk->ssat,msg)) {
        errmsg(rtk,"point pos error (%s)\n",msg);
        
        if (!rtk->opt.dynamics) {
            outsolstat(rtk);
            return 0;
        }
    }//计算当前历元和上一历元时间差rtk->tt，rtk->sol.time是当前历元时间，time是上一历元时间
    if (time.time!=0) rtk->tt=timediff(rtk->sol.time,time);
    if (fabs(rtk->tt)<DTTOL&&opt->mode<=PMODE_FIXED) return stat;

    /* single point positioning spp或spp_ins紧组合*/
    if (opt->mode==PMODE_SINGLE||tcs) {
        outsolstat(rtk);
        return 1;
    }
    /* suppress output of single solution抑制单个解的输出 */
    if (!opt->outsingle) {
        rtk->sol.stat=SOLQ_NONE;
    }
    /* precise point positioning ppp或ppp_ins紧组合*/
    if ((opt->mode>=PMODE_PPP_KINEMA&&opt->mode<PMODE_INS_UPDATE)||tcp) {
        pppos(rtk,opt->adjobs?obsd:obs,nu,nav);
        outsolstat(rtk);
        return 1;
    }//检查该历元流动站观测时间和基准站观测时间是否对应
    /* 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;
    }//动基线与其他差分定位方式，动基线的基站坐标需要随时间同步变化，所以需要计算出变化速率
    if (opt->mode==PMODE_MOVEB) { /*  moving baseline */
        //解释了为什么第二步除了单点定位，动基线也不参与基站解算，动基线在这里单独解算
        /* estimate position/velocity of base station */
        if (!pntpos(opt->adjobs?obsd:obs+nu,nr,nav,&rtk->opt,&solb,NULL,NULL,NULL,msg)) {
            errmsg(rtk,"base station position error (%s)\n",msg);
            return 0;
        }//计算差分龄期(流动站历元-基站历元)
        rtk->sol.age=(float)timediff(rtk->sol.time,solb.time);
        
        if (fabs(rtk->sol.age)>TTOL_MOVEB) {//差分龄期>1.01s
            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;
        }
    }
    /* relative positioning *///上面的步骤只算了相对定位的差分时间和动基线坐标,这里进行相位定位，并输出最终结果
    stat=relpos(rtk,opt->adjobs?obsd:obs,nu,nr,nav);

#if DEGRADETC
    /* degrade to dgps-tc mode if rtk-tc fail如果rtk-tc失败，则降级为dgps-tc模式 */
    if (stat==0&&opt->mode==PMODE_INS_TGNSS) {
        insopt->tc=INSTC_DGPS;
        stat=relpos(rtk,opt->adjobs?obsd:obs,nu,nr,nav);
        insopt->tc=INSTC_RTK;
        if (stat) goto exit;
    }
    /* degrade to single-tc mode if dgps-tc fail 如果dgps-tc失败，则降级为spp-tc模式*/
    if (stat==0&&opt->mode==PMODE_INS_TGNSS) {
        stat=pntpos(opt->adjobs?obsd:obs,nu,nav,&rtk->opt,&rtk->sol,&rtk->ins,NULL,NULL,msg);
        goto exit;
    }
exit:
#endif
    outsolstat(rtk);
    return stat;

其中关于RTKLIB的注释参考：

RTKLIB源码阅读（九）相对定位 RTK、PPK、RTD_rtklib源代码_李郑骁学导航的博客-CSDN博客

后文将不再详细阐述与RTKLIB中重复的部分，若有疑问，请参考上面的优秀文章

六、relpos()

以下大部分代码与原来的relpos()函数一致，只挑有区别的解析。

 prcopt_t *opt=&rtk->opt;
    insopt_t *insopt=&rtk->opt.insopt;
    insstate_t *ins=&rtk->ins;
    gtime_t time=obs[0].time;//流动站第一个观测卫星的时间
    ddsat_t ddsat[MAXSAT]={{0}};
    static insstate_t insp={0};
    static int refsat[NUMSYS][2*NFREQ]={0};
    double *Ri,*Rj,dr[3]={0};
    double *rs,*dts,*var,*y,*e,*azel;
    double *v,*H,*R,*xp,*Pp,*xa,*bias,dt,*x,*P,rr[3],*Pa,*dx;
    int i,j,k,f,n=nu+nr,ns,ny,nv=0,sat[MAXSAT],iu[MAXSAT],ir[MAXSAT],niter,nx,na;
    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,tc;
    int ix,iy,iz,ivx,ivy,ivz,namb=0;
    int nb[NFREQ*4*2+2]={0},b=0,m;

    /* tc=0: common rtk position mode
     * tc=1: tightly-coupled mode
     * */
    tc=opt->mode==PMODE_INS_TGNSS?1:0;
    nx=tc?ins->nx:rtk->nx;
    na=tc?ins->nb:rtk->na;

    x=tc?ins->x:rtk->x;//通过INS获取状态先验值
    P=tc?ins->P:rtk->P;//通过INS获取状态先验协方差矩阵

    trace(3,"relpos  : nx=%d nu=%d nr=%d\n",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);
    Ri=mat(n*nf*2+2,1); Rj=mat(n*nf*2+2,1);

    for (i=0;i<MAXSAT;i++) {
        rtk->ssat[i].sys=(unsigned char)satsys(i+1,NULL);//确定导航系统
        for (j=0;j<NFREQ;j++) rtk->ssat[i].vsat [j]=0;
        for (j=1;j<NFREQ;j++) rtk->ssat[i].snr  [j]=0;
        for (j=0;j<NFREQ;j++) rtk->ssat[i].vsatc[j]=0;
    }
    /* satellite positions/clocks根据卫星星历计算当前历元下所有卫星的位置、速度、钟差 */
    satposs(time,obs,n,nav,opt->sateph,rs,dts,var,svh);
    //计算基准站的各卫星观测值的非差残差（观测值-各项改正计算值）及卫星的高度角、方位角、卫星矢量等
    /* undifferenced residuals for base station */
    if (!zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,svh+nu,nav,rtk->rb,opt,1,
               y+nu*nf*2,e+nu*3,azel+nu*2)) {
        errmsg(rtk,"initial base station position error\n");
        
        free(rs); free(dts); free(var);
        free(y); free(e); free(azel);
        free(Ri); free(Rj);
        return 0;
    }//后处理中，需要时，调用 intpres 对基站进行插值
    /* time-interpolation of residuals (for post-processing) */
    if (opt->intpref) {
        dt=intpres(time,obs+nu,nr,nav,rtk,y+nu*nf*2);
    }//选择基准站和流动站之间的同步观测卫星。进行rtk算法时只需要基线间的同步观测卫星
    /* 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");
        //返回共同观测的卫星个数，输出卫星号列表sat、在流动站观测值中的index值列表iu和在基站观测值中的index值列表ir
        free(rs); free(dts); free(var);
        free(y); free(e); free(azel);
        free(Ri); free(Rj);
        return 0;
    }
    /* temporal update of states 更新状态值rtk->x及其误差协方差 rtk->P*/
    udstate(rtk,obs,sat,rs,iu,ir,ns,nav);
    
    xp=zeros(nx,1); xa=zeros(nx,1); Pp=zeros(nx,nx);
    dx=zeros(nx,1);

    /* backup estimated estates */
    matcpy(xp,x,nx,1);
    if (tc) {
        for (i=0;i<xnCl(insopt);i++) xp[i]=1E-10;
    }
    ny=ns*nf*2+2;//共视卫星*频率*2+2,双差残差向量维数
    v=mat(ny,1); H=zeros(nx,ny);
    R=mat(ny,ny); bias=mat(nx,1);

    /* backup ins states反馈ins状态 */
    memcpy(&insp,ins,sizeof(insstate_t));
    
    /* add 2 iterations for baseline-constraint moving-base设置迭代次数niter，动基线加2次 */
    niter=opt->niter+(opt->mode==PMODE_MOVEB&&opt->baseline[0]>0.0?2:0);

    for (i=0;i<(tc?1:niter);i++) {//紧组合时，只迭代一次
        tc?insp2antp(&insp,rr):matcpy(rr,xp,1,3);//IMU载体坐标转换到GNSS天线坐标

        /* undifference residuals for rover 对流动站进行非差残差计算*/
        if (!zdres(0,obs,nu,rs,dts,svh,nav,rr,opt,0,y,e,azel)) {
            errmsg(rtk,"rover initial position error\n");
            stat=SOLQ_NONE;
            break;
        }
        /* double-difference residuals and partial derivatives双差残差和偏导数 */
        if ((nv=ddres(rtk,nav,obs,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,H,R,vflg,rr,
                      NULL,NULL,NULL,NULL,refsat))<1) {
            errmsg(rtk,"no double-difference residual\n");
            stat=SOLQ_NONE;
            break;
        }
        /* kalman filter measurement update */
        matcpy(Pp,P,nx,nx);

        if ((info=filter(xp,Pp,H,v,R,nx,nv))) {
            errmsg(rtk,"filter error (info=%d)\n",info);
            stat=SOLQ_NONE;
            break;
        }
        /* close loop for ins state *///ins状态闭环
        if (tc) {
            clp(&insp,insopt,xp);
            if (i==0) {
                matcpy(dx,xp,1,xnCl(insopt));
            }
            for (j=0;j<xnCl(insopt);j++) {
                xp[j]=1E-10;
            }
        }
    }
    tc?insp2antp(&insp,rr):matcpy(rr,xp,1,3);
//计算双差相位/码残差
    if (stat!=SOLQ_NONE&&zdres(0,obs,nu,rs,dts,svh,nav,rr,opt,0,y,e,azel)) {
        
        /* post-fit residuals for float solution通过浮点解获取后验残差 */
        nv=ddres(rtk,nav,obs,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,NULL,
                 R,vflg,rr,Ri,Rj,nb,&b,refsat);
        //计算流动站非差、双差闭合差，进行浮点解有效性验证
        /* validation of float solution */
        if (nv&&valpos(rtk,v,R,vflg,nv,&m,4.0)&&(tc?valins(opt,dx):true)) {
            
            /* update state and covariance matrix */
            matcpy(P,Pp,nx,nx);
            matcpy(x,xp,nx, 1);
            
            /* 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 */
            }
            if (rtk->sol.ns<3) {

                /* valid satellite count by L1-pseudorange */
                for (rtk->sol.ns=0,i=0;i<ns;i++) {
                    for (f=0;f<nf;f++) if (rtk->ssat[sat[i]-1].vsatc[f]) break;
                    rtk->sol.ns++;
                }
            }
            /* lack of valid satellites 检验卫星是否少于4颗*/
            if (rtk->sol.ns<3) {

                /* tightly-coupled is available though lack satellite */
                if (tc) stat=SOLQ_FLOAT; else stat=SOLQ_NONE;

                /* degrade to pseudorange-dgps */
                if (m>=4&&stat==SOLQ_NONE) {
                    rtk->sol.ns=(unsigned char)m;
                    stat=SOLQ_DGPS;
                }
            }
        }
        else stat=SOLQ_NONE;
    }
    /* resolve integer ambiguity by WL-NL */
    if (stat!=SOLQ_NONE&&rtk->opt.modear==ARMODE_WLNLC) {

        if (resamb_WLNL(rtk,obs,sat,iu,ir,ns,nav,azel)) {
            stat=SOLQ_FIX;
        }
    }
    /* resolve integer ambiguity by TCAR */
    else if (stat!=SOLQ_NONE&&rtk->opt.modear==ARMODE_TCARC) {

        if (resamb_TCAR(rtk,obs,sat,iu,ir,ns,nav,azel)) {
            stat=SOLQ_FIX;
        }
    }
    /* resolve integer ambiguity by LAMBDA */
    else if (stat!=SOLQ_NONE&&resamb_LAMBDA(rtk,bias,xa,ddsat,&namb,vflg,nv)>1) {

        if (tc) {
            /* close loop for ins states */
            clp(&insp,insopt,xa);

            /* convert to gps antenna position */
            insp2antp(&insp,rr);
        }
        else {
            matcpy(rr,xa,1,3);
        }
        /* check solutions */
        if (zdres(0,obs,nu,rs,dts,svh,nav,rr,opt,0,y,e,azel)) {
            
            /* post-fit residuals for fixed solution */
            nv=ddres(rtk,nav,obs,dt,xa,NULL,sat,y,e,azel,iu,ir,ns,v,NULL,R,vflg,rr,NULL,NULL,
                     NULL,NULL,NULL);
            
            /* validation of fixed solution */
            if (nv&&valpos(rtk,v,R,vflg,nv,&m,4.0)&&(tc?valins(opt,xa):true)) {
                
                /* hold integer ambiguity */
                if (++rtk->nfix>=rtk->opt.minfix&&
                    rtk->opt.modear>=ARMODE_FIXHOLD) {
                    holdamb(rtk,&insp,xa,ddsat,namb);
                }
                /* store integer ambiguity */
                storeddamb(rtk,ddsat,namb,bias);

                stat=SOLQ_FIX;
            }
        }
    }

其中：

    /* tc=0: common rtk position mode
     * tc=1: tightly-coupled mode
     * */
    tc=opt->mode==PMODE_INS_TGNSS?1:0;
    nx=tc?ins->nx:rtk->nx;//若为紧组合模式，nx为紧组合模型状态参数
    na=tc?ins->nb:rtk->na;//na为流动站状态参数

    x=tc?ins->x:rtk->x;//通过INS获取状态先验值
    P=tc?ins->P:rtk->P;//通过INS获取状态先验协方差矩阵

注意紧组合模式与非组合模式在

   /* backup ins states反馈ins状态 */
    memcpy(&insp,ins,sizeof(insstate_t));

将INS的先验状态结构体ins复制到insp，insp在后续解算得到后验状态。

    for (i=0;i<(tc?1:niter);i++) {//紧组合时，只迭代一次,我猜测ins先验值精度比spp获得先验位置精度高很多，不需要多次迭代。
        tc?insp2antp(&insp,rr):matcpy(rr,xp,1,3);//IMU载体坐标转换到GNSS天线坐标

通过GNSS观测值与代入（IMU转换得到的近似值状态参数）的函数做差，正式引入IMU状态参数，进入紧组合核心阶段。

        /* undifference residuals for rover */
        if (!zdres(0,obs,nu,rs,dts,svh,nav,rr,opt,0,y,e,azel)) {
            errmsg(rtk,"rover initial position error\n");
            stat=SOLQ_NONE;
            break;
        }

在此处用GNSS观测值-（IMU转换的位置求得的卫地距+各项改正）即为非差残差，在此处对GNSS观测方程熟悉的应该会注意到，整周模糊度的物理意义是卫星发出信号到接收机首次锁定载波相位时的卫地距对应的相位数的整数部分，而GNSS相位观测值也并非当前历元的卫地距对应的相位数，因此GNSS观测值-（IMU转换的位置求得的卫地距+各项改正）为负数，且是一个很大的负数。

        /* double-difference residuals and partial derivatives */
        if ((nv=ddres(rtk,nav,obs,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,H,R,vflg,rr,
                      NULL,NULL,NULL,NULL,refsat))<1) {
            errmsg(rtk,"no double-difference residual\n");
            stat=SOLQ_NONE;
            break;
        }

在双差残差函数ddres()处还会涉及到ins相关状态参数对应设计矩阵的赋值，下文着重介绍。

通过卡尔曼滤波估计后，得到浮点状态参数

     /* post-fit residuals for float solution通过浮点解获取后验残差 */
        nv=ddres(rtk,nav,obs,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,NULL,
                 R,vflg,rr,Ri,Rj,nb,&b,refsat);

回带进ddres()函数，获取更新后的双差残差向量。

if (nv&&valpos(rtk,v,R,vflg,nv,&m,4.0)&&(tc?valins(opt,dx):true)) 

经过有效性验证函数valpos()和valins()函数之后，保存模糊度信息并统计有效卫星数。

if (rtk->sol.ns<3) {

                /* tightly-coupled is available though lack satellite */
                if (tc) stat=SOLQ_FLOAT; else stat=SOLQ_NONE;

                /* degrade to pseudorange-dgps */
                if (m>=4&&stat==SOLQ_NONE) {
                    rtk->sol.ns=(unsigned char)m;
                    stat=SOLQ_DGPS;
                }
            }

当有效卫星不足3颗时，如果是紧组合模式，直接输出浮点解，这也是紧组合的特色。

进行完模糊度固定模式后，更新完最新的历元之后，更新新的卫星。

#if UPDNEWSAT  //更新状态，观测值，观测误差
    /* update state by new satellites */
    for (k=0,i=0;i<nv;i++) {
        Rj[i]=SQR(100.0);

        /* check observation type */
        if (((vflg[i]>>4)& )==1) continue;//如果卫星使用过，则跳过
        j=( vflg[i]>>8)&0xFF;//获取卫星编号
        f=((vflg[i]   )&0xF);//&0xF操作用于获取频率信息，用于区分不同的 GNSS 频率
        if (rtk->ssat[j-1].news[f]) {//如果卫星已经使用过，则跳过
            continue;
        }

            /* new satellite 计算新卫星的测量误差方差*/
            Rj[i]=varerr(j,rtk->ssat[j-1].sys,rtk->ssat[j-1].azel[1],baseline(rr,rtk->rb,dr),dt,f,opt);
            k++;//递增k，表示发现了一个新卫星
            continue;
        }
    }

>> 8 操作用于获取卫星编号的高位，&0xFF 用于确保取得的值在一个字节的范围内

这段代码通过检查观测到的卫星信息，判断是否有新卫星，如果有的话，计算新卫星的测量误差方差，并将其存储在 Rj 数组。

  if (k) {//发现新卫星
        /* measurement error covariance计算测量误差协方差矩阵R*/
        ddcov(nb,b,Ri,Rj,nv,R);
        if (tc) {//将状态向量x的某些元素初始化为非常小的值
            for (j=0;j<xnCl(insopt);j++) {
                x[j]=1E-10;
            }
        }
        /* kalman filter */
        if ((info=filter(x,P,H,v,R,nx,nv))) {
            errmsg(rtk,"filter error (info=%d)\n",info);
            stat=SOLQ_NONE;
        }
        if (tc) {
            /* close loop for ins states 对惯导系统的状态进行闭环处理*/
            clp(&insp,insopt,x);
        }
    }

之后便根据固定解和浮点解模式输出保存结果。

1、zdres()

/* undifferenced phase/code residuals //计算非差残差----------------------*/
static int zdres(int base, const obsd_t *obs, int n, const double *rs,
                 const double *dts, const int *svh, const nav_t *nav,
                 const double *rr, const prcopt_t *opt, 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;//残差置0
    //若没有接收机位置
    if (norm(rr,3)<=0.0) return 0; /* no receiver position */
    
    for (i=0;i<3;i++) rr_[i]=rr[i];//接收机位置传给 rr_
    //若需要地球潮校正，调用 tidedisp 对 rr_ 进行校正。地球潮包含固体潮、极潮和海潮负荷
    /* earth tide correction */
    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);//大地坐标转空间直角坐标
    //遍历观测量
    for (i=0;i<n;i++) {//遍历卫星
        /* compute geometric-range and azimuth/elevation angle //根据卫星位置和接收机位置计算卫地距，并进行地球自转改正*/
        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?//排除需要排除的卫星 */
        if (satexclude(obs[i].sat,svh[i],opt)) continue;
        
        /* satellite clock-bias//对卫地距进行卫星钟差距离改正r*/
        r+=-CLIGHT*dts[i*2];
        //对卫地距进行对流层延迟改正，利用Saastamoinen模型改正对流层的干延迟，湿延迟通过随机游走估计的方式进行改正
        /* troposphere delay model (hydrostatic) */
        zhd=tropmodel(obs[0].time,pos,zazel,0.0);
        r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd;
        //接收机天线相位中心改正，调用 antmodel 计算校正值 dant（对每一个频率都有一个值）
        /* receiver antenna phase center correction */
        antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1],
                 dant);
        //观测值减经过上述各项改正后的计算值，
        /* undifferenced phase/code residual for satellite */
        zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2);
    }//得到最终的非差闭合差v。此处涉及到无电离层组合观测值的求解。
    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、selsat()

/* select common satellites between rover and reference station 选择共视卫星--*/
static int selsat(const obsd_t *obs, double *azel, int nu, int nr,
                  const prcopt_t *opt, int *sat, int *iu, int *ir)
{
    int i,j,k=0;
    
    trace(3,"selsat  : nu=%d nr=%d\n",nu,nr);
    //i为流动站卫星索引，j为基准站的；
    for (i=0,j=nu;i<nu&&j<nu+nr;i++,j++) {
        if      (obs[i].sat<obs[j].sat) j--;//卫星序号大的，自减1
        else if (obs[i].sat>obs[j].sat) i--;
        else if (azel[1+j*2]>=opt->elmin) { /* elevation at base station */
            sat[k]=obs[i].sat; iu[k]=i; ir[k++]=j;//存储卫星号以及索引
            trace(4,"(%2d) sat=%3d iu=%2d ir=%2d\n",k-1,obs[i].sat,i,j);
        }
    }
    return k;
}

3、udstate()

实现状态向量的时间更新，包括位置/速度/加速度、电离层参数、对流层参数、接收机硬件偏差和载波相位偏差的时间更新过程

/* temporal update of states --------------------------------------------------*/
static void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat, const double *rs,
                    const int *iu, const int *ir, int ns, const nav_t *nav)
{
    register int tc;
    double tt=rtk->tt,bl,dr[3];
    
    trace(3,"udstate : ns=%d\n",ns);//当前共视卫星

    /* temporal update of position/velocity/acceleration 更新位置速度加速度*/
    rtk->opt.mode==PMODE_INS_TGNSS?insudsta(rtk,tt):udpos(rtk,tt);

    /* temporal update of ionospheric parameters *///更新iono参数
    if (rtk->opt.ionoopt>=IONOOPT_EST) {//iono参数估计
        bl=baseline(rtk->x,rtk->rb,dr);//计算baseline基线
        udion(rtk,tt,bl,sat,ns);
    }
    /* temporal update of tropospheric parameters *///更新troposphere参数
    if (rtk->opt.tropopt>=TROPOPT_EST) {
        bl=baseline(rtk->x,rtk->rb,dr);
        udtrop(rtk,tt,bl);
    }
    /* temporal update of receiver h/w bias *///更新接收机硬件偏差
    if (rtk->opt.glomodear==2&&(rtk->opt.navsys&SYS_GLO)) {
        udrcvbias(rtk,tt);
    }
    /* tc-mode */
    tc=rtk->opt.mode==PMODE_INS_TGNSS&&rtk->opt.insopt.tc==INSTC_RTK;

    /* temporal update of phase-bias *///更新相位偏差
    if ((rtk->opt.mode>PMODE_DGPS&&rtk->opt.mode<PMODE_INS_UPDATE)||tc) {
        udbias(rtk,tt,obs,sat,rs,iu,ir,ns,nav);
    }
#if 1
    /* check counts of solution valid fail */
    if (rtk->failc>=MINFAILC) udreset(rtk);
#endif
}

insudsta()

static void insudsta(rtk_t *rtk,double tt)
{
    trace(3,"insudsta : tt=%6.4lf\n",tt);
}

看到此处没有实现代码，原因在状态先验值和状态方差阵通过IMU获得。

baseline()

static double baseline(const double *ru, const double *rb, double *dr)
{
    int i;
    for (i=0;i<3;i++) dr[i]=ru[i]-rb[i];//获取流动站与基站间的基线向量
    return norm(dr,3);
}

udioo()

double el,fact,*x=NULL;
    register int i,j,tc=0;
    insstate_t *ins=NULL;
    
    trace(3,"udion   : tt=%.3f bl=%.0f ns=%d\n",tt,bl,ns);

    ins=&rtk->ins;
    x=(tc=rtk->opt.mode==PMODE_INS_TGNSS?1:0)?rtk->ins.x:rtk->x;
    //遍历每颗卫星
    for (i=1;i<=MAXSAT;i++) {
        j=tc?xiIo(&rtk->opt.insopt,i):II(i,&rtk->opt);//获取电离层参数下标j
        if (x[j]!=0.0&&//如果两个频率，载波中断计数都大于GAP_RESION(120)，电离层参数设为0
            rtk->ssat[i-1].outc[0]>GAP_RESION&&
            rtk->ssat[i-1].outc[1]>GAP_RESION)
            x[j]=0.0;
    }
    for (i=0;i<ns;i++) {//遍历共视卫星
        j=tc?xiIo(&rtk->opt.insopt,sat[i]):II(sat[i],&rtk->opt);//获取电离层参数下标
 //如果电离层状态量为0，状态设为1E-6，协方差设为SQR(rtk->opt.std[1]*bl/1E4)，bl为基线长度       
        if (x[j]==0.0) {
            tc?
            insinitx(&rtk->ins,1E-6,SQR(rtk->opt.std[1]*bl/1E4),j):
            initx(rtk,1E-6,SQR(rtk->opt.std[1]*bl/1E4),j);
        }//电离层状态量不为0，加过程噪声SQR(rtk->opt.prn[1]*bl/1E4*fact)*fabs(tt)
        else {
            /* elevation dependent factor of process noise */
            el=rtk->ssat[sat[i]-1].azel[1];
            fact=cos(el);
            tc?
            ins->P[j+j*ins->nx]+=SQR(rtk->opt.prn[1]*bl/1E4*fact)*fabs(tt):
            rtk->P[j+j*rtk->nx]+=SQR(rtk->opt.prn[1]*bl/1E4*fact)*fabs(tt);
        }
    }

udtrop()

/* temporal update of tropospheric parameters --------------------------------*/
static void udtrop(rtk_t *rtk, double tt, double bl)
{
    register int i,j,k,tc=0,n;
    double *x=NULL,*P=NULL;
    insstate_t *ins=NULL;
    //处理基站、移动站对流层延迟
    trace(3,"udtrop  : tt=%.3f\n",tt);

    ins=&rtk->ins;
    tc=rtk->opt.mode==PMODE_INS_TGNSS;
    x=tc?rtk->ins.x:rtk->x;
    P=tc?rtk->ins.P:rtk->P;
    n=tc?rtk->ins.nx:rtk->nx;
    
    for (i=0;i<2;i++) {
        j=tc?xiTr((&rtk->opt.insopt),i):IT(i,&rtk->opt);//获取对流层参数下标j
        if (x[j]==0.0) {//如果对流层状态量为0
            tc?
            insinitx(ins,INIT_ZWD,SQR(rtk->opt.std[2]),j):
            initx(rtk,INIT_ZWD,SQR(rtk->opt.std[2]),j); /* initial zwd */
            //TROPOPT_ESTG模式，用1E-6、VAR_GRA初始化东向、北向的对流层梯度系数
            if (rtk->opt.tropopt>=TROPOPT_ESTG) {
                for (k=0;k<2;k++) {
                    tc?
                    insinitx(&rtk->ins,1E-6,VAR_GRA,++j):
                    initx(rtk,1E-6,VAR_GRA,++j);
                }
            }
        }
        else {//增加过程噪声SQR(rtk->opt.prn[2])*fabs(tt)
            P[j+j*n]+=SQR(rtk->opt.prn[2])*fabs(tt);
//TROPOPT_ESTG模式，给东向、北向的对流层梯度系数加过程噪声SQR(rtk->opt.prn[2]*0.3)*fabs(tt)
            if (rtk->opt.tropopt>=TROPOPT_ESTG) {
                for (k=0;k<2;k++) {
                    P[++j*(1+n)]+=SQR(rtk->opt.prn[2]*0.3)*fabs(tt);
                }
            }
        }
    }
}

与GNSS相关的状态参数及其误差方差阵更新与rtklib版本相比变化不大，后面的udrcvbias()、udbias()、udreset()函数对照电离层和对流层函数自行理解。

4、ddres()

static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, 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,const double *re,
                 double *Ri_,double *Rj_,int *nb_,int *b_,
                 int refsat[NUMSYS][2*NFREQ])
{
    prcopt_t *opt=&rtk->opt;
    insopt_t *insopt=&opt->insopt;
    double bl,dr[3],posu[3],posr[3],didxi,didxj,*im,*vc,ddi,ddg,factor=1.0;
    double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,lami,lamj,fi,fj,df,*Hi=NULL,rr[3];
    double dp[3]={0},da[3]={0},dl[3]={0},S[9],dap[3];
    int i,j,k,m,f,ff,nv=0,nb[NFREQ*4*2+2]={0},b=0,sysi,sysj,nf=NF(opt),tc,nx;
    int ii,ij,flag=0;

    trace(3,"ddres   : dt=%.1f ns=%d\n",dt,ns);
    trace(3,"base position=%.3lf %.3lf %.3lf\n",rtk->rb[0],rtk->rb[1],rtk->rb[2]);

    /* tc=0: common rtk mode
     * tc=1: tightly coupled mode
     * */
    tc=opt->mode==PMODE_INS_TGNSS?1:0;

    /* flag=0: dgps-tightly coupled mode伪距差分
     * flag=1: rtk-tightly coupled mode载波相位差分
     * */
    flag=opt->mode==PMODE_INS_TGNSS&&insopt->tc==INSTC_RTK;

    tc?nx=rtk->ins.nx:nx=rtk->nx;//状态参数个数
    tc?matcpy(rr,re,1,3):matcpy(rr,x,1,3);//流动站位置

    bl=baseline(rr,rtk->rb,dr);//计算基线长度bl，基线向量dr
    ecef2pos(rr,posu); ecef2pos(rtk->rb,posr);//基准站、流动站 XYZ-LLH

    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);
    vc=mat(ns*nf,1);

    for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++) {
        rtk->ssat[i].resp[j]=rtk->ssat[i].resc[j]=0.0;//所有rtk_t结构体内的伪距残差、载波相位残差重置为0
        rtk->ssat[i].news[j]=0;
    }
    /* compute factors of ionospheric and tropospheric delay */
    for (i=0;i<ns;i++) {//若 电离层模式>=IONOOPT_EST，调用 ionmapf() 计算电离层延迟因子
        if (opt->ionoopt>=IONOOPT_EST) {
            im[i]=(ionmapf(posu,azel+iu[i]*2)+ionmapf(posr,azel+ir[i]*2))/2.0;
        }//若 对流层模式>=TROPOPT_EST，调用 prectrop() 计算对流层延迟因子
        if (opt->tropopt>=TROPOPT_EST) {//基准站、流动站，im[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<4;m++) /* m=0:gps/qzs/sbs,1:glo,2:gal,3:bds */
    //0~nf-1为载波，nf~2*nf-1为伪距
    for (f=(opt->mode>PMODE_DGPS&&opt->mode<PMODE_INS_UPDATE)||flag?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 (f<nf) {//对所有载波相位观测值
                /* check all frequency phase-observation data */
                if (!chkfrq(obs+ir[j],opt)) continue;

                /* cycle slip */
                if (rtk->ssat[sat[j]-1].slip[f]) {
                    continue;
                }
            }
            if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) i=j;
        }
        if (i<0) continue; 

        /* double difference satellite 记录观测值所属的卫星*/
        if (refsat) refsat[m][f]=sat[i];

        /* make double difference */
        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 (!validobs(iu[j],ir[j],f,nf,y)) continue;//检查观测数据是否有效
            if (!test_sys(sysj,m)) continue;//检查卫星系统是否相同

            factor=1.0;

            if (f<nf) {
                /* check all frequency phase-observation data */
                if (!chkfrq(obs+iu[j],opt)) continue;

                /* cycle slip 发生周跳*/
                if (rtk->ssat[sat[j]-1].slip[f]) {
                    factor*=10.0;
                }
            }
            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*nx;
                for (k=0;k<nx;k++) Hi[k]=0.0;
            }//[参考星(移动站) - 参考星(基准站)]- [非参考星(移动站) - 非参考星(基准站)]
            /* double-differenced residual */
            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) {
                if (!tc) for (k=0;k<3;k++) {//非组合
                    Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3];//H矩阵每一列前三行为观测向量差
                }
                else {//紧组合
                    /* partial derivations by ins position H矩阵对应INS位置参数赋值*/
                    jacob_dd_dp(&rtk->ins,&e[iu[i]*3],&e[iu[j]*3],dp);
                    Hi[xiP(insopt)+0]=dp[0];
                    Hi[xiP(insopt)+1]=dp[1];
                    Hi[xiP(insopt)+2]=dp[2];
                }
            }
            /* partial derivation by ins attitude error H矩阵对应INS姿态参数赋值*/
            if (H&&tc) {
                jacob_dd_da(&rtk->ins,&e[iu[i]*3],&e[iu[j]*3],da);

#if UPD_IN_EULER
                jacobian_prot_pang(rtk->ins.Cbe,S);
                matcpy(dap,da,1,3);
                matmul("NN",1,3,3,1.0,dap,S,0.0,da);
#endif
                Hi[xiA(insopt)+0]=da[0];
                Hi[xiA(insopt)+1]=da[1];
                Hi[xiA(insopt)+2]=da[2];
            }
            /* partial derivation by lever arm H矩阵对应INS杆臂参数赋值*/
            if (H&&tc&&xnLa(insopt)) {
                jacob_dd_dl(&rtk->ins,&e[iu[i]*3],&e[iu[j]*3],dl);
                Hi[xiLa(insopt)+0]=dl[0];
                Hi[xiLa(insopt)+1]=dl[1];
                Hi[xiLa(insopt)+2]=dl[2];
            }
            /* double-differenced ionospheric delay term 估计电离层参数*/
            if (opt->ionoopt==IONOOPT_EST) {
                ii=tc?xiIo(insopt,sat[i]):II(sat[i],opt);
                ij=tc?xiIo(insopt,sat[j]):II(sat[j],opt);

                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]-didxj*x[ij];//电离层残差校正
                if (H) {
                    Hi[ii]=didxi; Hi[ij]=-didxj;//对应H矩阵
                }
            }
            /* double-differenced tropospheric delay term 估计对流层参数*/
            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;
                    ii=tc?xiTr(insopt,0):IT(0,opt);
                    ij=tc?xiTr(insopt,1):IT(1,opt);
                    Hi[ii+k]= (dtdxu[k+i*3]-dtdxu[k+j*3]);//对应H矩阵
                    Hi[ij+k]=-(dtdxr[k+i*3]-dtdxr[k+j*3]);
                }
            }
            /* double-differenced phase-bias term */
            if (f<nf) {//对载波相位观测值

                ii=tc?xiBs(insopt,sat[i],f):IB(sat[i],f,opt);
                ij=tc?xiBs(insopt,sat[j],f):IB(sat[j],f,opt);
                
                if (opt->ionoopt!=IONOOPT_IFLC) {//用相位偏移修正v和H
                    v[nv]-=lami*x[ii]-lamj*x[ij];
                    if (H) Hi[ii]=lami,Hi[ij]=-lamj;
                }
                else {//从载波相位双差残差中扣除双差模糊度部分（即phase-bias）
                    v[nv]-=x[ii]-x[ij];
                    if (H) Hi[ii]=1.0,Hi[ij]=-1.0;//并对H矩阵中和模糊度相关的部分进行赋值
                }
            }
            /* glonass receiver h/w bias term */
            if (rtk->opt.glomodear==2
                &&sysi==SYS_GLO&&sysj==SYS_GLO
                &&ff<NFREQGLO) {

                /* freq-difference (MHz) */
                df=(CLIGHT/lami-CLIGHT/lamj)/1E6;
                ii=tc?xiLl(insopt,ff):IL(ff,opt);
                
                v[nv]-=df*x[ii];
                if (H) Hi[ii]=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];

            if (f>=nf&&tc) {
                ddi=rtk->ssat[sat[i]-1].sdi[f%nf]-rtk->ssat[sat[j]-1].sdi[f%nf];
                ddg=rtk->ssat[sat[i]-1].sdg[f%nf]-rtk->ssat[sat[j]-1].sdg[f%nf];
                if (fabs(ddi-ddg)>1.0) continue;
            }
            /* 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;
            }//计算单差的测量误差协方差Ri、i为参考星，j为非参考星
            /* single-difference measurement error variances */
            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 *///设置数据有效标志，首先是载波，然后是伪距；根据f
            if (opt->mode>PMODE_DGPS&&opt->mode<PMODE_INS_UPDATE||flag) {
                if (f<nf) {
                    rtk->ssat[sat[i]-1].vsat [f]=rtk->ssat[sat[j]-1].vsat[f]=1;
                    rtk->ssat[sat[j]-1].index[f]=(unsigned int)nv;
                }
            }
            else {
                rtk->ssat[sat[i]-1].vsat[f-nf]=rtk->ssat[sat[j]-1].vsat[f-nf]=1;
            }
            /* code measurement data valid flag */
            if (f>=nf) {
                rtk->ssat[sat[i]-1].vsatc[f-nf]=rtk->ssat[sat[j]-1].vsatc[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]);
#if UPDNEWSAT
            /* check this phase double difference is new? */
            if (f<nf&&(rtk->refsat[m][f]&&sat[i]==rtk->refsat[m][f])&&chknews(rtk,sat[i],sat[j],f)) {
                trace(3,"new satellite come in,sat=%2d-%2d\n",sat[i],sat[j]);

                /* new double difference satellite flag */
                rtk->ssat[sat[j]-1].news[f]=1;

                /* disable double difference measurement */
                Rj[nv]=SQR(30.0);
            }
#endif
            Rj[nv]*=factor;
            vflg[nv++]=(sat[i]<<16)|(sat[j]<<8)|((f<nf?0:1)<<4)|(f%nf);
            nb[b]++;
        }
        b++;
    }
    /* end of system loop */
#if DETECT_OUTLIER
    static const double r0=re_norm(0.95),r1=re_norm(0.99);
    static double s0;
    //探测异常值
    /* detect outlier by L1/L2 phase double difference residual */
    if (nf>=2&&opt->mode>PMODE_DGPS) {
        double *vv=mat(ns,1),avv=0.0,v0=0.0;
        int l,*s=imat(ns,1);

        /* iteration for detect outliers */
        for (m=0,flag=1;m<ns;m++,flag=1) {//遍历共视卫星
            for (l=0,i=0;i<ns;i++) {
                if (!rtk->ssat[sat[i]-1].vsat[0]) continue;//有效卫星标志为00跳过

                /* exclude reference satellite 排除参考卫星*/
                if (rtk->refsat[sysind(rtk->ssat[sat[i]-1].sys)][0]==sat[i]) continue;
                if (rtk->refsat[sysind(rtk->ssat[sat[i]-1].sys)][1]==sat[i]) continue;

                /* L1-L2 */
                j=rtk->ssat[sat[i]-1].index[0];
                k=rtk->ssat[sat[i]-1].index[1];
                
                vv[l]=v[j]-v[k];//L1双差残差-L2双差残差

                /* index of dd-res */
                s[l++]=i;
            }
            if (l<3) break;//当有效卫星不足4颗时，跳出循环，l 为有效卫星个数

            /* outliers detect *///异常值检测
            for (i=0;i<l;i++) avv+=vv[i]; avv/=l;//(L1双差残差-L2双差残差)平均值
            for (i=0;i<l;i++) vv[i]-=avv;//(L1双差残差-L2双差残差)-平均值

            /* standard deviation *///标准差
            matmul("NT",1,1,l,1.0/(l-1),vv,vv,0.0,&v0);//(L1双差残差-L2双差残差)标准差

            /* chi-square detect outliers *///卡方检测异常值
            for (i=0;i<l;i++) {
                if (fabs(v0)>=THRES_L1L2RES&&fabs(vv[i])/SQRT(v0)>=r0) {

                    trace(2,"L1/L2 dd-res detect outlier,sat=%2d\n",sat[s[i]]);
                    j=rtk->ssat[sat[s[i]]-1].index[0];
                    k=rtk->ssat[sat[s[i]]-1].index[1];

                    /* disable this satellite */
                    Rj[j]=Rj[k]=SQR(100.0);

                    for (f=0;f<nf;f++) {
                        rtk->ssat[sat[s[i]]-1].vsat[f]=0;
                    }
                    /* outlier flag */
                    flag=0;
                }
            }
            if (flag) break;
        }
        free(s); free(vv);
    }
    /* code double-difference measurement outliers detect *///码双差测量异常值检测
    for (s0=0.0,k=0,i=0;i<nv;i++) {
        if (((vflg[i]>>4)&0xF)==0) continue;
        vc[k++]=v[i];
        s0+=v[i];
    }
    if (k>2) {
        s0/=k;
        for (i=0;i<k;i++) vc[i]-=s0;
        matmul("NT",1,1,k,1.0/(k-1),vc,vc,0.0,&s0);

        /* outliers detect */
        for (i=0,k=0;i<nv;i++) {
            if (((vflg[i]>>4)&0xF)==0) continue;
            if (fabs(vc[k])/SQRT(s0)>=r1) {

                /* disable this satellite */
                Rj[i]=SQR(100.0);
            }
            else if (fabs(vc[k])/SQRT(s0)>=r0) {

                /* degrade this satellite */
                Rj[i]=SQR(10.0);
            }
            k++;
        }
    }
#endif
    /* 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++]++;
    }
    /* double-differenced measurement error covariance计算载波相位/伪距双差量测噪声协方差阵R*/
    ddcov(nb,b,Ri,Rj,nv,R);

    /* output used variables if need */
    if (Ri_&&Rj_) {
        matcpy(Ri_,Ri,1,nv);
        matcpy(Rj_,Rj,1,nv);
    }
    if (nb_&&b_) {
        for (*b_=b,i=0;i<b;i++) nb_[i]=nb[i];
    }
    free(Ri); free(Rj); free(im);
    free(tropu); free(tropr);
    free(dtdxu); free(dtdxr);
    free(vc); return nv;
}


         }
                    /* outlier flag */
                    flag=0;
                }
            }
            if (flag) break;
        }
        free(s); free(vv);
    }
    /* code double-difference measurement outliers detect *///码双差测量异常值检测
    for (s0=0.0,k=0,i=0;i<nv;i++) {
        if (((vflg[i]>>4)&0xF)==0) continue;
        vc[k++]=v[i];
        s0+=v[i];
    }
    if (k>2) {
        s0/=k;
        for (i=0;i<k;i++) vc[i]-=s0;
        matmul("NT",1,1,k,1.0/(k-1),vc,vc,0.0,&s0);

        /* outliers detect */
        for (i=0,k=0;i<nv;i++) {
            if (((vflg[i]>>4)&0xF)==0) continue;
            if (fabs(vc[k])/SQRT(s0)>=r1) {

                /* disable this satellite */
                Rj[i]=SQR(100.0);
            }
            else if (fabs(vc[k])/SQRT(s0)>=r0) {

                /* degrade this satellite */
                Rj[i]=SQR(10.0);
            }
            k++;
        }
    }
#endif
    /* 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++]++;
    }
    /* double-differenced measurement error covariance计算载波相位/伪距双差量测噪声协方差阵R*/
    ddcov(nb,b,Ri,Rj,nv,R);

    /* output used variables if need */
    if (Ri_&&Rj_) {
        matcpy(Ri_,Ri,1,nv);
        matcpy(Rj_,Rj,1,nv);
    }
    if (nb_&&b_) {
        for (*b_=b,i=0;i<b;i++) nb_[i]=nb[i];
    }
    free(Ri); free(Rj); free(im);
    free(tropu); free(tropr);
    free(dtdxu); free(dtdxr);
    free(vc); return nv;
}