PPP/INS紧组合代码学习

前言：

本文是基于IGNAV的PPP/INS紧组合学习，在此之前需要具备GNSS/INS松组合知识，武汉大学的i2nav实验室的KF-GINS项目可以作为学习模板。可以参考这篇优秀博文，链接：KF-GINS源码阅读_李郑骁学导航的博客-CSDN博客 IGNAV是基于RTKLIB进行二次开发的，因此熟悉RTKLIB源代码的学习起来会容易一些。我的这篇博文也大概介绍了RTKLIB中PPP模块，可做简略学习：RTKLIB学习（二）–2、PPP代码分析-CSDN博客在学习一个项目之前，对算法的学习至关重要，对算法的理解程度很大程度上决定了你对代码的理解能力，因此IGNAV项目中man文件夹内的theory.pdf。算法文件中6.3.2-GNSS 伪距与载波观测信息辅助INS的过程推到，务必花时间理解。我的这篇文章总结了i2nav团队关于PPP/INS紧组合学习知识也可作为算法学习参考：PPP/INS紧组合算法-CSDN博客
本人学习能力有限，不足的地方请批评指正。

一、IGNAV的PPP/INS流程

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

在这里插入图片描述

updatetimediff()

 /* ins tightly coupled position mode */if (svr->rtk.opt.mode==PMODE_INS_TGNSS) {if (syn->ni&&syn->nr) {//IMU和流动站时间对齐索引均不为0syn->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);}}}

在PPP/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()匹配的观测值所在历元索引+iif (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历元内观测值数不为0for (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.imureturn 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;}}
#endifins->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)) {
#endiftrace(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   <n&&obs[nu   ].rcv==1;nu++) ;for (nr=0;nu+nr<n&&obs[nu+nr].rcv==2;nr++) ;dt=timediff(obs[0].time,ins->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_EULERgetphi_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()、pppos()

 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;}}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) {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];/* 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 */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 */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:
#endifoutsolstat(rtk);return stat;

需要注意到的是：此处状态参数x中的位置参数是IMU坐标系下的，而rr[]存储的是地心地固下的坐标。这里也是spp/ins,rtk/ins紧组合入口函数，注意区分。这里只介绍PPP/INS紧组合入口函数pppos()。pppos()代码在下面：

extern void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{const prcopt_t *opt=&rtk->opt;double *rs,*dts,*var,*v,*H,*R,*azel,*xp,*Pp,dr[3]={0},std[3];double *x,*P,rr[3];char str[32];int i,j,nv,info,svh[MAXOBS],exc[MAXOBS]={0},stat=SOLQ_SINGLE,tc;int nx;insstate_t insp;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);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);/* 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);}nv=n*rtk->opt.nf*2+MAXSAT+3;xp=zeros(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx);v=mat(nv,1); H=mat(rtk->nx,nv); R=mat(nv,nv);/* tc=0: common rtk mode* tc=1: tightly coupled mode* */tc=opt->mode==PMODE_INS_TGNSS?1:0;x=tc?rtk->ins.x:rtk->x;//通过INS解算的状态先验值P=tc?rtk->ins.P:rtk->P;//通过INS解算的参数协方差//若为紧组合模式，nx为紧组合模型状态参数nx=tc?rtk->ins.nx:rtk->nx;/* backup ins states ins状态参数反馈矩阵*/if (tc) {memcpy(&insp,&rtk->ins,sizeof(insstate_t));}for (i=0;i<MAX_ITER;i++) {//迭代//若为组合模式，将IMU位置坐标转换到GNSS天线位置tc?insp2antp(&insp,rr):matcpy(rr,xp,1,3);matcpy(xp,x,nx,1);matcpy(Pp,P,nx,nx);/* prefit residuals先验残差 */if (!(nv=ppp_res(0,obs,n,rs,dts,var,svh,dr,exc,nav,xp,rtk,v,H,R,azel,rr))) {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,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,rr)) {matcpy(x,xp,nx,1);matcpy(P,Pp,nx,nx);stat=SOLQ_PPP;if (tc) {clp(&insp,&opt->insopt,xp);for (j=0;j<xnCl(&opt->insopt);j++) xp[j]=0.0;}break;}}if (i>=MAX_ITER) {trace(2,"%s ppp (%d) iteration overflows\n",str,i);}if (stat==SOLQ_PPP) {/* update ins states if solution is ok */if (tc) {upd_ins_states(rtk,&insp);}tc?insp2antp(&insp,rr):matcpy(rr,xp,1,3);/* todo: add ppp-ar fix ambiguity *//* ambiguity resolution in ppp */if (ppp_ar(rtk,obs,n,exc,nav,azel,xp,Pp)&&ppp_res(9,obs,n,rs,dts,var,svh,dr,exc,nav,xp,rtk,v,H,R,azel,rr)) {double *xa,*Pa;xa=tc?rtk->ins.xb:rtk->xa;Pa=tc?rtk->ins.Pb:rtk->Pa;for (i=0;i<3;i++) std[i]=sqrt(Pp[i+i*nx]);if (norm(std,3)<MAX_STD_FIX) stat=SOLQ_FIX;if (stat==SOLQ_FIX) {if (tc) {clp(&insp,&opt->insopt,xp);for (j=0;j<xnCl(&opt->insopt);j++) xp[j]=0.0;/* update ins states if solution is ok */if (tc) {upd_ins_states(rtk,&insp);}}matcpy(xa,xp,nx, 1);matcpy(Pa,Pp,nx,nx);}}else {rtk->nfix=0;}/* update solution status */update_stat(rtk,obs,n,stat);/* hold fixed ambiguities */if (stat==SOLQ_FIX&&test_hold_amb(rtk)) {matcpy(x,xp,nx,1);matcpy(P,Pp,nx,nx);trace(2,"%s hold ambiguity\n",str);rtk->nfix=0;}}free(rs); free(dts); free(var); free(azel);free(xp); free(Pp); free(v); free(H); free(R);
}

其中udstate_ppp()、satposs()、testeclipe()、tidedisp()、pppar()、update_stat()、update_stat()并没有多大改变，不作阐述。

1、先验残差ppp_res()

 const double *lam;prcopt_t *opt=&rtk->opt;insopt_t *insopt=&opt->insopt;double y,r,cdtr,bias,C,rr[3],pos[3],e[3],dtdx[3],L[NFREQ],P[NFREQ],Lc,Pc;double var[MAXOBS*2],dtrp=0.0,dion=0.0,vart=0.0,vari=0.0,dcb;double dantr[NFREQ]={0},dants[NFREQ]={0},uddp[3],udda[3],uddl[3];double ve[MAXOBS*2*NFREQ]={0},vmax=0;char str[32];int ne=0,obsi[MAXOBS*2*NFREQ]={0},frqi[MAXOBS*2*NFREQ],maxobs,maxfrq,rej;int i,j,k,sat,sys,nv=0,nx=rtk->nx,stat=1,tc;/* tc=0: common rtk mode* tc=1: tightly coupled mode* */tc=opt->mode==PMODE_INS_TGNSS?1:0;time2str(obs[0].time,str,2);//将第一个观测历元的时间转换为字符串，存储在str中。//将 rtk->ssat 中的卫星状态标记为未使用for (i=0;i<MAXSAT;i++) for (j=0;j<opt->nf;j++) rtk->ssat[i].vsat[j]=0;for (i=0;i<3;i++) rr[i]=rpos[i]+dr[i];//dr[]为地球潮汐对坐标改正量ecef2pos(rr,pos);for (i=0;i<n&&i<MAXOBS;i++) {sat=obs[i].sat;lam=nav->lam[sat-1];//波长if (lam[j/2]==0.0||lam[0]==0.0) continue;if ((r=geodist(rs+i*6,rr,e))<=0.0||satazel(pos,e,azel+i*2)<opt->elmin) {exc[i]=1;continue;}if (!(sys=satsys(sat,NULL))||!rtk->ssat[sat-1].vs||satexclude(obs[i].sat,svh[i],opt)||exc[i]) {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;}/* 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);/* 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);/* stack phase and code residuals {L1,P1,L2,P2,...} */for (j=0;j<2*NF(opt);j++) {dcb=bias=0.0;if (opt->ionoopt==IONOOPT_IFLC) {if ((y=j%2==0?Lc:Pc)==0.0) continue;}else {if ((y=j%2==0?L[j/2]:P[j/2])==0.0) continue;/* receiver DCB correction for P2 */if (j/2==1) dcb=-nav->rbias[0][sys==SYS_GLO?1:0][0];}C=SQR(lam[j/2]/lam[0])*ionmapf(pos,azel+i*2)*(j%2==0?-1.0:1.0);if (tc) {//IMU位置状态参数---对观测方程xcom对应的雅克比矩阵jacob_ud_dp(e,uddp);//e[]为卫星到接收机的向量除以几何距离H[xiP(insopt)+0]=uddp[0];H[xiP(insopt)+1]=uddp[1];H[xiP(insopt)+2]=uddp[2];}else {for (k=0;k<nx;k++) H[k+nx*nv]=k<3?-e[k]:0.0;}/* partial derivation by ins attitude error利用ins姿态误差进行部分推导 */if (H&&tc) {jacob_ud_da(e,rtk->ins.lever,rtk->ins.Cbe,udda);H[xiA(insopt)+0]=udda[0];H[xiA(insopt)+1]=udda[1];H[xiA(insopt)+2]=udda[2];}/* partial derivation by lever arm 杆臂偏导数*/if (H&&tc) {jacob_ud_la(e,rtk->ins.Cbe,uddl);H[xiLa(insopt)+0]=uddl[0];H[xiLa(insopt)+1]=uddl[1];H[xiLa(insopt)+2]=uddl[2];}/* receiver clock接收机钟差雅克比矩阵系数 */k=sys==SYS_GLO?1:(sys==SYS_GAL?2:(sys==SYS_CMP?3:0));/* xiRc(insopt)+0: GPS receiver clock* xiRc(insopt)+1: GLO receiver clock* xiRc(insopt)+2: GAL receiver clock* xiRc(insopt)+3: BDS receiver clock* */cdtr=x[tc?xiRc(insopt)+k:IC(k,opt)];H[tc?xiRc(insopt)+k:IC(k,opt)+nx*nv]=1.0;/* todo: ppp/ins tightly coupled */if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) {for (k=0;k<(opt->tropopt>=TROPOPT_ESTG?3:1);k++) {H[tc?xiTr(insopt,k):IT(opt)+k+nx*nv]=dtdx[k];//对流层湿延迟雅克比矩阵系数}}if (opt->ionoopt==IONOOPT_EST) {//电离层估计int ii=tc?xiIo(insopt,sat):II(sat,opt);if (rtk->x[ii]==0.0) continue;H[ii+nx*nv]=C;}if (j/2==2&&j%2==1) { /* L5-receiver-dcb */dcb+=rtk->x[tc?xiDl(insopt):ID(opt)];H[tc?xiDl(insopt):ID(opt)+nx*nv]=1.0;}if (j%2==0) { /* phase bias 相位偏差*/int ib=tc?xiBs(insopt,sat,j/2):IB(sat,j/2,opt);if ((bias=x[ib])==0.0) continue;H[ib+nx*nv]=1.0;}/* residual y是GNSS观测值*/v[nv]=y-(r+cdtr-CLIGHT*dts[i*2]+dtrp+C*dion+dcb+bias);if (j%2==0) rtk->ssat[sat-1].resc[j/2]=v[nv];else        rtk->ssat[sat-1].resp[j/2]=v[nv];/* variance */var[nv]=varerr(obs[i].sat,sys,azel[1+i*2],j/2,j%2,opt)+vart+SQR(C)*vari+var_rs[i];if (sys==SYS_GLO&&j%2==1) var[nv]+=VAR_GLO_IFB;trace(4,"%s sat=%2d %s%d res=%9.4f sig=%9.4f el=%4.1f\n",str,sat,j%2?"P":"L",j/2+1,v[nv],sqrt(var[nv]),azel[1+i*2]*R2D);/* reject satellite by pre-fit residuals用先验残差排除卫星 */if (!post&&opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {trace(2,"outlier (%d) rejected %s sat=%2d %s%d res=%9.4f el=%4.1f\n",post,str,sat,j%2?"P":"L",j/2+1,v[nv],azel[1+i*2]*R2D);exc[i]=1; rtk->ssat[sat-1].rejc[j%2]++;continue;}/* record large post-fit residuals记录大的后验残差 */if (post&&fabs(v[nv])>sqrt(var[nv])*THRES_REJECT) {obsi[ne]=i; frqi[ne]=j; ve[ne]=v[nv]; ne++;}if (j%2==0) rtk->ssat[sat-1].vsat[j/2]=1;nv++;}}/* reject satellite with large and max post-fit residual 排除卫星*/if (post&&ne>0) {vmax=ve[0]; maxobs=obsi[0]; maxfrq=frqi[0]; rej=0;for (j=1;j<ne;j++) {if (fabs(vmax)>=fabs(ve[j])) continue;vmax=ve[j]; maxobs=obsi[j]; maxfrq=frqi[j]; rej=j;}sat=obs[maxobs].sat;trace(2,"outlier (%d) rejected %s sat=%2d %s%d res=%9.4f el=%4.1f\n",post,str,sat,maxfrq%2?"P":"L",maxfrq/2+1,vmax,azel[1+maxobs*2]*R2D);exc[maxobs]=1; rtk->ssat[sat-1].rejc[maxfrq%2]++; stat=0;ve[rej]=0;}/* constraint to local correction 局部校正约束*/nv+=const_corr(obs,n,exc,nav,x,pos,azel,rtk,v+nv,H+nv*rtk->nx,var+nv);for (i=0;i<nv;i++) for (j=0;j<nv;j++) {R[i+j*nv]=i==j?var[i]:0.0;//观测值方差}return post?stat:nv;

如果对自己的要求比较高，需要了解雅克比矩阵H中矩阵块赋值情况，建议对i2nav团队上传在bilibili上的组合导航最后一章：PPP/INS紧组合进行详细的学习。注意在细节上此项目与视频有差异。此项目貌似并没有使用一步解法（存疑）。

2、测量更新扩展卡尔曼滤波

算法详情;RTKLIB学习(二)–1、PPP方程和扩展卡尔曼滤波等算法详解-CSDN博客

extern int filter(double *x, double *P, const double *H, const double *v,const double *R, int n, int m)

需要注意到的是：此处输入的*x为ins解算出来的先验状态参数，解算出来的状态参数x中的位置参数是IMU坐标系下的。但是在KF-GINS中扩展卡尔曼滤波中的状态参数是改正量。

3、后验残差

if (ppp_res(i+1,obs,n,rs,dts,var,svh,dr,exc,nav,xp,rtk,v,H,R,azel,rr))

ppp_res()是通过第一个传入的参数区分先验还是后验。在后验函数中主要做了检验工作，并排除故障卫星。