修复RTK/INS紧组合在卫星信号较弱的地方出现的Bugs

Erensu · May 31, 2019 · 99f4794 · 99f4794
1 parent d013144
commit 99f4794
Show file tree

Hide file tree

Showing 6 changed files with 29 additions and 11 deletions.
diff --git a/include/carvig.h b/include/carvig.h
@@ -59,7 +59,7 @@ extern "C"{
 #else
 #define EXPORT
 #endif
-//#define ENACMP
+#define ENACMP
 #define ENAOPENCV   0                   /* enable opencv library */
 
 /* constants -----------------------------------------------------------------*/

diff --git a/src/ins-gnss/app/carvig-tc.conf b/src/ins-gnss/app/carvig-tc.conf
@@ -24,7 +24,7 @@ pos1-posopt4       =on         # (0:off,1:on)
 pos1-posopt5       =on         # (0:off,1:on)
 pos1-posopt6       =off        # (0:off,1:on)
 pos1-exclsats      =           # (prn ...)
-pos1-navsys        =1         # (1:gps+2:sbas+4:glo+8:gal+16:qzs+32:comp)
+pos1-navsys        =33         # (1:gps+2:sbas+4:glo+8:gal+16:qzs+32:comp)
 pos1-adjobs        =1          # adjust observation data
 pos1-eps           =1          # time internal between epochs
 pos1-gtfmt         =1          # ground truth solution format

diff --git a/src/ins-gnss/ins-gnss-tc.cc b/src/ins-gnss/ins-gnss-tc.cc
@@ -55,7 +55,7 @@ static int chkvb(const insstate_t *ins)
 /* initialization position mode/ionosphere and troposphere option------------*/
 static void initrtkpos(rtk_t *rtk,prcopt_t *prcopt)
 {
-    prcopt->mode   =PMODE_SINGLE;
+    prcopt->mode   =PMODE_KINEMA;
     prcopt->ionoopt=IONOOPT_BRDC;
     prcopt->tropopt=TROPOPT_SAAS;
     rtkinit(rtk,prcopt);

diff --git a/src/ins-gnss/ins-pose.cc b/src/ins-gnss/ins-pose.cc
@@ -18,7 +18,7 @@
 
 /* constants ----------------------------------------------------------------*/
 #define ORDERS        10           /* orders of approximate exponential of matrix */
-#define MAXRES_POSE        10.0*D2R     /* max residual for pose filter */
+#define MAXRES_POSE   10.0*D2R     /* max residual for pose filter */
 #define VARPOSE       30.0*D2R     /* variance of pose measurement */
 #define MAXTIMEDIFF   1.0          /* max difference between ins and pose measurement time */
 #define JACOB_LEFT    1            /* use left jacobians of misalignment of camera to imu */

diff --git a/src/ins-gnss/pntpos.cc b/src/ins-gnss/pntpos.cc
@@ -457,7 +457,7 @@ static int itertc(const obsd_t *obs,int n,const double *rs,const double *dts,
         x[i]=1E-20;
     }
     /* tightly coupled */
-    if (nv>5) {
+    if (nv>4) {
 
         /* measurement variance */
         for (i=0;i<nv;i++) R[i+i*nv]=var[i];

diff --git a/src/ins-gnss/rtkpos.cc b/src/ins-gnss/rtkpos.cc
@@ -40,6 +40,7 @@
 *-----------------------------------------------------------------------------*/
 #include <stdarg.h>
 #include <carvig.h>
+#include <include/carvig.h>
 
 /* constants/macros ----------------------------------------------------------*/
 #define VAR_POS     SQR(30.0)  /* initial variance of receiver pos (m^2) */
@@ -2992,6 +2993,9 @@ extern void rtkinit(rtk_t *rtk, const prcopt_t *opt)
                 case INSLC_RTK   : rtk->opt.mode=PMODE_KINEMA    ; break;
             }
         }
+        if (opt->mode==PMODE_INS_TGNSS) {
+            fprintf(stderr,"best choice for dual frequency\n");
+        }
         rtk->ins.rtkp=rtk;
     }
     else {
@@ -3023,11 +3027,17 @@ extern void rtkfree(rtk_t *rtk)
     if (rtk->ins.xb) free(rtk->ins.xb); rtk->ins.xb=NULL;
     if (rtk->ins.xa) free(rtk->ins.xa); rtk->ins.xa=NULL;
 
-    if (rtk->ins.gmeas.data) free(rtk->ins.gmeas.data); rtk->ins.gmeas.data=NULL;
+    if (rtk->ins.gmeas.data) {
+        free(rtk->ins.gmeas.data);
+    }
+    rtk->ins.gmeas.data=NULL;
     rtk->ins.nx=rtk->ins.nb=0;
     rtk->ins.gmeas.n=rtk->ins.gmeas.nmax=0;
 
-    if (rtk->bias.amb) free(rtk->bias.amb); rtk->bias.amb=NULL;
+    if (rtk->bias.amb) {
+        free(rtk->bias.amb);
+    }
+    rtk->bias.amb=NULL;
 }
 /* precise positioning ---------------------------------------------------------
 * input observation data and navigation message, compute rover position by
@@ -3113,8 +3123,7 @@ extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
     if (n<=0) return 0;
 
     /* set base staion position */
-    if (opt->refpos<=POSOPT_RINEX&&opt->mode!=PMODE_SINGLE&&
-        opt->mode!=PMODE_MOVEB) {
+    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;
         }
@@ -3125,8 +3134,9 @@ extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
 
     /* for rover and base observation data */
     for (i=0;i<nu+nr&&opt->adjobs;i++) {
-
         memcpy(&obsd[i],&obs[i],sizeof(obsd_t));
+
+        /* adjust observation */
         if (adjsind(opt,&obs[i],&fi,&fj,&fk)) {
             trace(4,"adjust observation data signal index ok\n");
         }
@@ -3156,10 +3166,18 @@ extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
                 NULL,rtk->ssat,msg)) {
         errmsg(rtk,"point pos error (%s)\n",msg);
 
-        if (!rtk->opt.dynamics) {
+        if (!rtk->opt.dynamics&&opt->mode<=PMODE_PPP_FIXED) {
             outsolstat(rtk);
             return 0;
         }
+        if (opt->mode==PMODE_INS_TGNSS) {
+            stat=pntpos(opt->adjobs?obsd:obs,nu,nav,
+                        &rtk->opt,
+                        &rtk->sol,
+                        &rtk->ins,
+                        NULL,NULL,msg);
+            goto exit;
+        }
     }
     if (time.time!=0) rtk->tt=timediff(rtk->sol.time,time);
     if (fabs(rtk->tt)<DTTOL&&opt->mode<=PMODE_FIXED) return stat;