添加IMU噪声建模方式，支持一阶高斯马尔可夫

TC_NAV_Zjut_post/Src/real_time/rt_ins-gnss.c

@@ -9,6 +9,9 @@
 #define MAXUPDTIMEINT       60.0                   /* max update time internal is same as correlation time */
 #define MAXVAR              1E10                   /* max variance for reset covariance matrix */
 #define UNC_CLK             (100.0)                /* default initial receiver clock uncertainty (m) */
+#define INS_RANDOM_WALK     1
+#define INS_GAUSS_MARKOV    2
+#define CORRETIME           360                    /* correlation time for gauss-markov process */
 #define LG69T
 
 /* insopt init ------------------------------------------*/
@@ -46,6 +49,8 @@ extern void rt_insopt_init(insopt_t *insopt){
     insopt->L_ba_b[0]=0.3;
     insopt->L_ba_b[1]=0.5;
     insopt->L_ba_b[2]=-0.8;
+
+    insopt->noise_type = INS_GAUSS_MARKOV;
 #elif
 
 #endif
@@ -300,11 +305,14 @@ static void setphi(double *Cbe, double *fb, double *ecef_pos, double dt, double
 }
 
 /* propagate matrix for stochastic parameters--------------------------------*/
-static void stochasticPhi(int ix,int nix,int nx,double dt,double *phi)
+static void stochasticPhi(int ix,int nix,int nx,double dt,double *phi,int opt)
 {
     int i;
     for (i=ix;i<ix+nix;i++) {
+        if(opt==INS_RANDOM_WALK)
             phi[i+i*nx]=1.0;
+        if(opt==INS_GAUSS_MARKOV)
+            phi[i+i*nx]=exp(-fabs(dt)/CORRETIME);
     }
 }
 
@@ -361,9 +369,8 @@ static void getPhi1(const int nx, double dt, const double *Cbe,
             phi[index_clk+i+(index_dtrr+i)*nx]=dt;
         }
     }
-
+    stochasticPhi(index_ba,num_ba,nx,dt,phi,insopt->noise_type);
-    stochasticPhi(index_ba,num_ba,nx,dt,phi);
+    stochasticPhi(index_bg,num_bg,nx,dt,phi,insopt->noise_type);
-    stochasticPhi(index_bg,num_bg,nx,dt,phi);
 }
 static void getQ(const insopt_t *insopt,double dt,double* Q,const int nx)
 {
@@ -444,8 +451,6 @@ static void updstat(const insopt_t *insopt,ins_t *ins,const double dt,
                     double *x,double *Q,const int upd)
 {
     int nx = xnX(insopt);
-//    setphi(ins->Cbe,ins->fb,ins->position,dt,phi,nx);
-//    setQ(insopt,dt,nx,Q,upd);
 
     getPhi1(nx,dt,ins->Cbe,ins->position,ins->omgb,ins->fb,phi,insopt);
     getQ(insopt,dt,Q,nx);
@@ -649,16 +654,15 @@ extern int tcigpos(const prcopt_t *opt,const obsd_t *obs,int n,const nav_t *nav,
     ins->stat = INSS_NONE;
     /* update ins states in e-frame */
     /* dt 在这里取值 */
-    if(ins->clk[0]!=1E-20||ins->clk[1]!=1E-20||ins->clk[2]!=1E-20||upd==INSUPD_MEAS){
     if(!updateins(ins,imu)){
         trace(2,"ins mechanization update fail\n");
         return 0;
     }
-    }
 
-//    propinss(ins,insopt,ins->dt,ins->x,ins->P,upd);
+
+    propinss(ins,insopt,ins->dt,ins->x,ins->P,upd);
     /* check variance of estimated position */
-//    chkpcov(nx,insopt,ins->P);
+    chkpcov(nx,insopt,ins->P);
 
 
     /* updates ins states using imu or observation data */
@@ -667,10 +671,6 @@ extern int tcigpos(const prcopt_t *opt,const obsd_t *obs,int n,const nav_t *nav,
         info=1;
     }
     else if(upd==INSUPD_MEAS){
-        propinss(ins,insopt,1.0,ins->x,ins->P,upd);
-//        /* check variance of estimated position */
-//        chkpcov(nx,insopt,ins->P);
-
         for (int i=0;i<6;i++) rtk->sol.pqr[i]=rtk->sol.qr[i];
         rtk->sol.pstat=rtk->sol.stat;
         ins->gstat=SOLQ_NONE;

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

@@ -1,3 +1,3 @@
-Start testing: Mar 29 15:11 CST
+Start testing: Mar 30 10:14 CST
 ----------------------------------------------------------
-End testing: Mar 29 15:11 CST
+End testing: Mar 30 10:14 CST

TC_NAV_Zjut_post/include/rtklib.h

@@ -732,6 +732,7 @@ typedef struct {
     double L_ba_b[3];               /* lever arm from antenna to imu */
     void *gopt;                     /* pointer to option */
 
+    int noise_type;                 /* INS_RANDOM_WALK or INS_GAUSS_MARKOV */
 }insopt_t;