添加计算电离层、对流层状态向量索引与数目的函数。修复Xnx函数在SPP/INS情况下未返回值的bug

TC_NAV_Zjut_post/Src/real_time/rt_ins-gnss.c

@@ -65,9 +65,10 @@ extern int xnIF(const insopt_t *insopt) {
 extern int xnX(const insopt_t *insopt) {
     switch (insopt->tc) {
         case INSTC_RTK:
-            return xnP(insopt)+ xnV(insopt)+ xnA(insopt)+ xnBa(insopt)+xnBg(insopt)+xnB(insopt);
+            return xnP(insopt)+ xnV(insopt)+ xnA(insopt)+ xnBa(insopt)+xnBg(insopt)+xnI(insopt)+
+                    xnT(insopt)+xnB(insopt);
         case INSTC_SINGLE:
-            xnP(insopt)+ xnV(insopt)+ xnA(insopt)+ xnBa(insopt)+xnBg(insopt)+xnRb(insopt)+xnRd(insopt);
+            return xnP(insopt)+ xnV(insopt)+ xnA(insopt)+ xnBa(insopt)+xnBg(insopt)+xnRb(insopt)+xnRd(insopt);
     }
 }
 /* get number of position */
@@ -90,6 +91,22 @@ extern int xnRd(const insopt_t *insopt){
     return ((insopt)->gopt?((prcopt_t*)(insopt)->gopt)->mode==PMODE_INS_TGNSS
                     &&(insopt)->tc<=INSTC_RTK?3:0:0);
 }
+/* get number of iono,trop,phase bias and h/w bias states are non-close-loop
+ * correction states
+ * --------------------------------------------------------------------------*/
+extern int xnI(const insopt_t *insopt)
+{
+    return ((insopt)->gopt?((prcopt_t*)(insopt)->gopt)->ionoopt!=IONOOPT_EST?0:MAXSAT:0);
+}
+extern int xnT(const insopt_t *insopt)
+{
+    return ((insopt)->gopt?((prcopt_t*)(insopt)->gopt)->tropopt<TROPOPT_EST?0:
+                        (((prcopt_t*)(insopt)->gopt)->tropopt<TROPOPT_ESTG?2:6):0);
+}
+/* get number of close loop states */
+extern int xnCl(const insopt_t *insopt){
+    return xnP(insopt)+ xnA(insopt)+ xnV(insopt)+ xnBa(insopt)+ xnBg(insopt);
+}
 /* index of position */
 extern int xiP(const insopt_t *insopt)  {return 6;}
 /* index of velocity */
@@ -106,7 +123,16 @@ extern int xiRb(const insopt_t *insopt) {return 15;}
 extern int xiRd(const insopt_t *insopt) {return 18;}
 /* index of phase bias */
 extern int xiBs(const insopt_t *insopt,int s,int f) {
-    return xnP(insopt)+ xnV(insopt)+ xnA(insopt)+ xnBa(insopt)+xnBg(insopt)+MAXSAT*f+s-1;
+    return xnP(insopt)+xnV(insopt)+xnA(insopt)+xnBa(insopt)+xnBg(insopt)+xnI(insopt)+
+            xnT(insopt)+MAXSAT*f+s-1;
+}
+/* get index of ionos (s:satellite no)---------------------------------------*/
+extern int xiIo(const insopt_t *insopt,int s){
+    return xnP(insopt)+xnV(insopt)+xnA(insopt)+xnBa(insopt)+xnBg(insopt)+(s-1);
+}
+/* get index of tropos (r:0=rov,1:ref)---------------------------------------*/
+extern int xiTr(const insopt_t *insopt,int r){
+    return xnP(insopt)+xnV(insopt)+xnA(insopt)+xnBa(insopt)+xnBg(insopt)+xnI(insopt)+xnT(insopt)/2*r;
 }
 /* system noise covariance matrix--------------------------------------------*/
 static void sysQ(int is,int n,int nx,double v,double dt,double *Q)
@@ -663,7 +689,6 @@ extern int tcigpos(const prcopt_t *opt,const obsd_t *obs,int n,const nav_t *nav,
             if (info) {
                 info=rtkpos(rtk,obs,n,nav);
             }
-
         }
         else info=0;
 

TC_NAV_Zjut_post/Src/rtkpos.c

@@ -923,10 +923,10 @@ static void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat,
 
     trace(3,"udstate : ns=%d\n",ns);
 
-    tc=rtk->opt.insopt.tc;
+    tc=rtk->opt.mode==PMODE_INS_TGNSS?1:0;
     
     /* temporal update of position/velocity/acceleration */
-    if(tc<INSTC_RTK){
+    if(!tc){
         udpos(rtk,tt);
     }
     /* temporal update of ionospheric parameters */
@@ -1223,12 +1223,17 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
     double bl,dr[3],posu[3],posr[3],didxi=0.0,didxj=0.0,*im,icb,threshadj;
     double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,freqi,freqj,*Hi=NULL,df,rr[3],dp[3]={0};
     int i,j,k,m,f,nv=0,nb[NFREQ*4*2+2]={0},b=0,sysi,sysj,nf=NF(opt);
-    int ii,jj,frq,code,nx;
+    int ii,jj,frq,code,nx,tc;
 
     trace(3,"ddres   : dt=%.1f nx=%d ns=%d\n",dt,rtk->nx,ns);
 
-    if(rtk->opt.insopt.tc){
-        matcpy(rr,rtk->ins.x,1,3);
+    /* tc=0: common rtk mode
+    *  tc=1: tightly coupled mode
+    * */
+    tc=opt->mode==PMODE_INS_TGNSS?1:0;
+
+    if(tc){
+        ins_2_ant(&rtk->ins,rr);
         nx=rtk->ins.nx;
     }
     else{
@@ -1295,8 +1300,8 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
                 if (!validobs(iu[j],ir[j],f,nf,y)) continue;
             
                 if (H) {
-                    Hi=H+nv*rtk->nx;
-                    for (k=0;k<rtk->nx;k++) Hi[k]=0.0;
+                    Hi=H+nv*nx;
+                    for (k=0;k<nx;k++) Hi[k]=0.0;
                 }
             
                 /* double-differenced measurements from 2 receivers and 2 sats in meters */
@@ -1306,7 +1311,7 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
             
                 /* partial derivatives by rover position, combine unit vectors from two sats */
                 if (H) {
-                    if(rtk->opt.insopt.tc){
+                    if(tc){
                         /* h(x)/x */
                         /* partial derivations by ins position */
                         jacob_dd_dp(&rtk->ins,&e[iu[i]*3],&e[iu[j]*3],dp);
@@ -1318,13 +1323,14 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
                     for (k=0;k<3;k++) {
                         Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3];  /* translation of innovation to position states */
                     }
-
                 }
                 if (opt->ionoopt==IONOOPT_EST) {
                     /* adjust double-differenced measurements by double-differenced ionospheric delay term */
+                    ii=tc?xiIo(&rtk->opt.insopt,sat[i]):II(sat[i],opt);
+                    jj=tc?xiIo(&rtk->opt.insopt,sat[j]):II(sat[j],opt);
                     didxi=(code?-1.0:1.0)*im[i]*SQR(FREQL1/freqi);
                     didxj=(code?-1.0:1.0)*im[j]*SQR(FREQL1/freqj);
-                    v[nv]-=didxi*x[II(sat[i],opt)]-didxj*x[II(sat[j],opt)];
+                    v[nv]-=didxi*x[ii]-didxj*x[jj];
                     if (H) {
                         Hi[II(sat[i],opt)]= didxi;
                         Hi[II(sat[j],opt)]=-didxj;
@@ -1335,13 +1341,15 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
                     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]);
+                        ii=tc?xiTr(&rtk->opt.insopt,0):IT(0,opt);
+                        jj=tc?xiTr(&rtk->opt.insopt,1):IT(1,opt);
+                        Hi[ii+k]= (dtdxu[k+i*3]-dtdxu[k+j*3]);
+                        Hi[jj+k]=-(dtdxr[k+i*3]-dtdxr[k+j*3]);
                     }
                 }
                 /* index of phase bias */
-                ii=IB(sat[i],frq,opt);
-                jj=IB(sat[j],frq,opt);
+                ii=tc?xiBs(&rtk->opt.insopt,sat[i],frq):IB(sat[i],frq,opt);
+                jj=tc?xiBs(&rtk->opt.insopt,sat[j],frq):IB(sat[j],frq,opt);
                 if (!code) {
                     /* adjust phase residual by double-differenced phase-bias term,
                           IB=look up index by sat&freq */
@@ -1391,10 +1399,13 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
                 if (code) rtk->ssat[sat[j]-1].resp[frq]=v[nv];  /* pseudorange */
                 else      rtk->ssat[sat[j]-1].resc[frq]=v[nv];  /* carrier phase */
 
+
+                double *P_;
+                P_=tc?rtk->ins.P:rtk->P;
                 /* if residual too large, flag as outlier */
                 /* adjust threshold by error stdev ratio unless one of the phase biases was just initialized*/
-                threshadj=code||(rtk->P[ii+rtk->nx*ii]==SQR(rtk->opt.std[0]))||
-                        (rtk->P[jj+rtk->nx*jj]==SQR(rtk->opt.std[0]))?opt->eratio[frq]:1;
+                threshadj=code||(P_[ii+nx*ii]==SQR(rtk->opt.std[0]))||
+                        (P_[jj+nx*jj]==SQR(rtk->opt.std[0]))?opt->eratio[frq]:1;
                 if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno*threshadj) {
                     rtk->ssat[sat[j]-1].vsat[frq]=0;
                     rtk->ssat[sat[j]-1].rejc[frq]++;
@@ -1427,7 +1438,7 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
                     icb=rtk->ssat[sat[i]-1].icbias[frq]*CLIGHT/freqi - rtk->ssat[sat[j]-1].icbias[frq]*CLIGHT/freqj;
                 trace(3,"sat=%3d-%3d %s%d v=%13.3f R=%9.6f %9.6f icb=%9.3f lock=%5d x=%9.3f\n",sat[i],
                         sat[j],code?"P":"L",frq+1,v[nv],Ri[nv],Rj[nv],icb,
-                        rtk->ssat[sat[j]-1].lock[frq],rtk->x[IB(sat[j],frq,&rtk->opt)]);
+                        rtk->ssat[sat[j]-1].lock[frq],tc?rtk->ins.x[xiBs(&rtk->opt.insopt,sat[j],frq)]:rtk->x[IB(sat[j],frq,&rtk->opt)]);
 
                 vflg[nv++]=(sat[i]<<16)|(sat[j]<<8)|((code?1:0)<<4)|(frq);
                 nb[b]++;
@@ -1441,7 +1452,7 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
         vflg[nv++]=3<<4;
         nb[b++]++;
     }
-    if (H) {trace(5,"H=\n"); tracemat(5,H,rtk->nx,nv,7,4);}
+    if (H) {trace(5,"H=\n"); tracemat(5,H,nx,nv,7,4);}
     
     /* double-differenced measurement error covariance */
     ddcov(nb,b,Ri,Rj,nv,R);
@@ -1947,7 +1958,7 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
 {
     prcopt_t *opt=&rtk->opt;
     gtime_t time=obs[0].time;
-    double *rs,*dts,*var,*y,*e,*azel,*freq,*v,*H,*R,*xp,*Pp,*xa,*bias,dt,*x;
+    double *rs,*dts,*var,*y,*e,*azel,*freq,*v,*H,*R,*xp,*Pp,*xa,*bias,dt,*x,*P,rr[3]={0};
     int i,j,f,n=nu+nr,ns,ny,nv,sat[MAXSAT],iu[MAXSAT],ir[MAXSAT],niter;
     int info,vflg[MAXOBS*NFREQ*2+1],svh[MAXOBS*2];
     int stat=rtk->opt.mode<=PMODE_DGPS?SOLQ_DGPS:SOLQ_FLOAT;
@@ -1957,6 +1968,7 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
     tc=opt->mode==PMODE_INS_TGNSS?1:0;
     nx=tc?rtk->ins.nx:rtk->nx;
     x=tc?rtk->ins.x:rtk->x;
+    P=tc?rtk->ins.P:rtk->P;
 
     /* time diff between base and rover observations */
     dt=timediff(time,obs[nu].time);
@@ -2027,15 +2039,23 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
     xa=mat(nx,1);
 //    matcpy(xp,rtk->x,nx,1);
     matcpy(xp,x,nx,1);
-
-
+    if (tc) {
+        for (i=0;i<xnCl(&rtk->opt.insopt);i++) xp[i]=1E-10;
+    }
     ny=ns*nf*2+2;
     v=mat(ny,1); H=zeros(nx,ny); R=mat(ny,ny); bias=mat(nx,1);
-    
+
     /* add 2 iterations for baseline-constraint moving-base  (else default niter=1) */
     niter=opt->niter+(opt->mode==PMODE_MOVEB&&opt->baseline[0]>0.0?2:0);
-    
+    /* tc 只进行一次迭代 */
     for (i=0;i<tc==1?1:niter;i++) {
+
+        if(tc){
+            ins_2_ant(&rtk->ins,rr);
+        }
+        else{
+            matcpy(rr,xp,1,3);
+        }
         /* calculate zero diff residuals [measured pseudorange - range] for rover (phase and code)
             output is in y[0:nu-1], only shared input with base is nav 
                 obs  = sat observations
@@ -2050,7 +2070,7 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
                 e    = line of sight unit vectors to sats
                 azel = [az, el] to sats                                   */
         trace(3,"rover:\n");
-        if (!zdres(0,obs,nu,rs,dts,var,svh,nav,xp,opt,0,y,e,azel,freq)) {
+        if (!zdres(0,obs,nu,rs,dts,var,svh,nav,rr,opt,0,y,e,azel,freq)) {
             errmsg(rtk,"rover initial position error\n");
             stat=SOLQ_NONE;
             break;
@@ -2076,8 +2096,8 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
                 K=P*H*(H'*P*H+R)^-1
                 xp=x+K*v
                 Pp=(I-K*H')*P                  */
-        matcpy(Pp,rtk->P,rtk->nx,rtk->nx);
-        if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) {
+        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;

TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log

@@ -1,3 +1,3 @@
-Start testing: Feb 02 14:21 CST
+Start testing: Feb 19 15:05 CST
 ----------------------------------------------------------
-End testing: Feb 02 14:21 CST
+End testing: Feb 19 15:05 CST

TC_NAV_Zjut_post/include/rtklib.h

@@ -1610,6 +1610,10 @@ EXPORT int xnBa(const insopt_t *insopt);
 EXPORT int xnBg(const insopt_t *insopt);
 EXPORT int xnRb(const insopt_t *insopt);
 EXPORT int xnRd(const insopt_t *insopt);
+EXPORT int xnI(const insopt_t *insopt);
+EXPORT int xnT(const insopt_t *insopt);
+EXPORT int xnCl(const insopt_t *insopt);
+
 EXPORT int xiP(const insopt_t *insopt);
 EXPORT int xnIF(const insopt_t *opt);
 EXPORT int xiV(const insopt_t *insopt);
@@ -1617,6 +1621,9 @@ EXPORT int xiA(const insopt_t *insopt);
 EXPORT int xiBa(const insopt_t *insopt);
 EXPORT int xiBg(const insopt_t *insopt);
 EXPORT int xiBs(const insopt_t *insopt,int s,int f);
+EXPORT int xiIo(const insopt_t *insopt,int s);
+EXPORT int xiTr(const insopt_t *insopt,int r);
+
 EXPORT void rt_insopt_init(insopt_t *insopt);
 EXPORT void initP(int is,int ni,int nx,double unc,double unc0,double *P0);
 EXPORT int ant2ins(gtime_t time, const double *rr, const double *vr,