From ddbfc707bf94b8e8d824a416e432503ac396c0b9 Mon Sep 17 00:00:00 2001 From: ZhiQiang_Yang98 <565093967qq.com> Date: Thu, 30 Mar 2023 11:13:27 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0IMU=E5=99=AA=E5=A3=B0?= =?UTF-8?q?=E5=BB=BA=E6=A8=A1=E6=96=B9=E5=BC=8F=EF=BC=8C=E6=94=AF=E6=8C=81?= =?UTF-8?q?=E4=B8=80=E9=98=B6=E9=AB=98=E6=96=AF=E9=A9=AC=E5=B0=94=E5=8F=AF?= =?UTF-8?q?=E5=A4=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- TC_NAV_Zjut_post/Src/real_time/ins.c | 8 ++-- TC_NAV_Zjut_post/Src/real_time/rt_ins-gnss.c | 46 +++++++++---------- .../Testing/Temporary/LastTest.log | 4 +- TC_NAV_Zjut_post/include/rtklib.h | 1 + 4 files changed, 30 insertions(+), 29 deletions(-) diff --git a/TC_NAV_Zjut_post/Src/real_time/ins.c b/TC_NAV_Zjut_post/Src/real_time/ins.c index a313180..1267393 100644 --- a/TC_NAV_Zjut_post/Src/real_time/ins.c +++ b/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->fbp ,ins->fb ,1,3); + matcpy(ins->omgbp ,ins->omgb ,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->pCbe ,ins->Cbe ,3,3); + matcpy(ins->p_velocity ,ins->velocity ,1,3); + matcpy(ins->pCbe ,ins->Cbe ,3,3); } /* calculates acceleration due to gravity resolved about ecef-frame --------- diff --git a/TC_NAV_Zjut_post/Src/real_time/rt_ins-gnss.c b/TC_NAV_Zjut_post/Src/real_time/rt_ins-gnss.c index de9eb37..e9c902f 100644 --- a/TC_NAV_Zjut_post/Src/real_time/rt_ins-gnss.c +++ b/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 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 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 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;inoise_type); + stochasticPhi(index_bg,num_bg,nx,dt,phi,insopt->noise_type); } 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; - } + 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; diff --git a/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log b/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log index 1d3da27..7c67329 100644 --- a/TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log +++ b/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 diff --git a/TC_NAV_Zjut_post/include/rtklib.h b/TC_NAV_Zjut_post/include/rtklib.h index bf73f3a..f542b15 100644 --- a/TC_NAV_Zjut_post/include/rtklib.h +++ b/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;