Wakanda_shaw/ZJUT_Nav
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
ZhiQiang_Yang98 2023-03-30 11:13:27 +08:00
parent d023a5d4c9
commit ddbfc707bf
4 changed files with 30 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
TC_NAV_Zjut_post/Src/real_time/rt_ins-gnss.c
View File

@ -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);
stochasticPhi(index_bg,num_bg,nx,dt,phi);
stochasticPhi(index_ba,num_ba,nx,dt,phi,insopt->noise_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;
}
}
// 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;

4
TC_NAV_Zjut_post/cmake-build-debug/Testing/Temporary/LastTest.log
View File

@ -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

1
TC_NAV_Zjut_post/include/rtklib.h
View File

@ -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;