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

TC_NAV_Zjut_post/Src/real_time/ins.c

@@ -276,11 +276,11 @@ extern void rotscull_corr(ins_t *ins,double dt,
 /* save precious epoch ins states--------------------------------------------*/
 static void savepins(ins_t *ins)
 {
-    matcpy(ins->omgbp ,ins->omgb,1,3);
+    matcpy(ins->omgbp       ,ins->omgb      ,1,3);
-    matcpy(ins->fbp   ,ins->fb  ,1,3);
+    matcpy(ins->fbp         ,ins->fb        ,1,3);
     matcpy(ins->p_position  ,ins->position  ,1,3);
-    matcpy(ins->p_velocity,ins->velocity  ,1,3);
+    matcpy(ins->p_velocity  ,ins->velocity  ,1,3);
-    matcpy(ins->pCbe  ,ins->Cbe ,3,3);
+    matcpy(ins->pCbe        ,ins->Cbe       ,3,3);
 }
 
 /* calculates  acceleration due to gravity resolved about ecef-frame ---------

TC_NAV_Zjut_post/Src/real_time/rt_ins-gnss.c

@@ -3,12 +3,15 @@
 //
 #include "rtklib.h"
 
-#define MAXDT       3600.0                 /* max time difference for ins-gnss coupled */
+#define MAXDT               3600.0                 /* max time difference for ins-gnss coupled */
 
-#define MICRO_G_2_MS  9.80665E-6      /* micro_g_to_meters_per_second_squared */
+#define MICRO_G_2_MS        9.80665E-6             /* micro_g_to_meters_per_second_squared */
-#define MAXUPDTIMEINT   60.0               /* max update time internal is same as correlation time */
+#define MAXUPDTIMEINT       60.0                   /* max update time internal is same as correlation time */
-#define MAXVAR       1E10         /* max variance for reset covariance matrix */
+#define MAXVAR              1E10                   /* max variance for reset covariance matrix */
-#define UNC_CLK     (100.0)                /* default initial receiver clock uncertainty (m) */
+#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++) {
-        phi[i+i*nx]=1.0;
+        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)){
-        if(!updateins(ins,imu)){
+        trace(2,"ins mechanization update fail\n");
-            trace(2,"ins mechanization update fail\n");
+        return 0;
-            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;