From 2d3d98f0911ad41c4401eacc1e61bde198f192fa Mon Sep 17 00:00:00 2001 From: ZhiQiang_Yang98 <565093967qq.com> Date: Fri, 3 Feb 2023 14:58:51 +0800 Subject: [PATCH] =?UTF-8?q?=E8=A7=84=E8=8C=83=E7=A8=8B=E5=BA=8F=E5=8F=98?= =?UTF-8?q?=E9=87=8F=E5=91=BD=E5=90=8D=EF=BC=8C=E9=92=88=E5=AF=B9rtk/ins?= =?UTF-8?q?=E4=B8=8Espp/ins=E6=83=85=E5=86=B5=E5=81=9A=E5=87=BA=E5=8D=B4?= =?UTF-8?q?=E5=88=AB=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- TC_NAV_Zjut_post/Src/real_time/rt_ins-gnss.c | 98 ++++++++++++++----- TC_NAV_Zjut_post/Src/rtkpos.c | 24 +++-- .../Testing/Temporary/LastTest.log | 4 +- TC_NAV_Zjut_post/include/rtklib.h | 11 +++ 4 files changed, 98 insertions(+), 39 deletions(-) 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 7542ea2..5336b9b 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 @@ -51,17 +51,59 @@ extern void rt_insopt_init(insopt_t *insopt){ #endif } - -/* index if position */ +/* get number of phase bias */ +extern int xnB(const insopt_t *insopt){ + return ((insopt)->gopt?((prcopt_t*)(insopt)->gopt)->mode==PMODE_INS_TGNSS + &&(insopt)->tc==INSTC_RTK?(MAXSAT*xnIF(insopt)):0:0); +} +/* numebr of observation data frequency */ +extern int xnIF(const insopt_t *insopt) { + return ((insopt)->gopt?((prcopt_t*)(insopt)->gopt)->ionoopt==IONOOPT_IFLC?1: + ((prcopt_t*)insopt->gopt)->nf:0); +} +/* get number of all states */ +extern int xnX(const insopt_t *insopt) { + switch (insopt->tc) { + case INSTC_RTK: + return xnP(insopt)+ xnV(insopt)+ xnA(insopt)+ xnBa(insopt)+xnBg(insopt)+xnB(insopt); + case INSTC_SINGLE: + xnP(insopt)+ xnV(insopt)+ xnA(insopt)+ xnBa(insopt)+xnBg(insopt)+xnRb(insopt)+xnRd(insopt); + } +} +/* get number of position */ +extern int xnP(const insopt_t *insopt) {return 3;} +/* get number of velocity */ +extern int xnV(const insopt_t *insopt) {return 3;} +/* get number of attitude */ +extern int xnA(const insopt_t *insopt) {return 3;} +/* get number of acc bias */ +extern int xnBa(const insopt_t *insopt){return 3;} +/* get number of gyro bias */ +extern int xnBg(const insopt_t *insopt){return 3;} +/* get numebr of receiver clk bias */ +extern int xnRb(const insopt_t *insopt){ + return ((insopt)->gopt?((prcopt_t*)(insopt)->gopt)->mode==PMODE_INS_TGNSS + &&(insopt)->tc<=INSTC_RTK?3:0:0); +} +/* get numer of receiver clk drift */ +extern int xnRd(const insopt_t *insopt){ + return ((insopt)->gopt?((prcopt_t*)(insopt)->gopt)->mode==PMODE_INS_TGNSS + &&(insopt)->tc<=INSTC_RTK?3:0:0); +} +/* index of position */ extern int xiP(const insopt_t *insopt) {return 6;} -/* index if velocity */ +/* index of velocity */ extern int xiV(const insopt_t *insopt) {return 3;} -/* index if attitude */ +/* index of attitude */ extern int xiA(const insopt_t *insopt) {return 0;} -/* index if acc bias */ +/* index of acc bias */ extern int xiBa(const insopt_t *insopt) {return 9;} -/* index if gyro bias */ +/* index of gyro bias */ extern int xiBg(const insopt_t *insopt) {return 12;} +/* index of receiver clk bias */ +extern int xiRb(const insopt_t *insopt) {return 15;} +/* index of receiver clk drift */ +extern int xiRd(const insopt_t *insopt) {return 18;} /* system noise covariance matrix--------------------------------------------*/ static void sysQ(int is,int n,int nx,double v,double dt,double *Q) { @@ -241,12 +283,15 @@ static void stochasticPhi(int ix,int nix,int nx,double dt,double *phi) /* determine transition matrix(first-order approx: Phi=I+F*dt)---------------*/ static void getPhi1(const int nx, double dt, const double *Cbe, const double *pos, const double *omgb, const double *fib, - double *phi) { + double *phi,const insopt_t *insopt) + { int i, j; double omega[3] = {0}, T[9], ge[3], re, rn[3], Cbv[9]; double W2[9]; - int index_att = 0,num_att=3,index_bg=12,num_bg=3,index_vel=3,num_vel=3,index_pos=6,num_pos=3; - int index_ba=9,num_ba=3,index_clk=15,num_clk=3,index_dtrr=18; + int index_att = xiA(insopt),num_att=xnA(insopt),index_bg=xiBg(insopt),num_bg=xnBg(insopt); + int index_vel= xiV(insopt),num_vel= xnV(insopt),index_pos= xiP(insopt),num_pos= xnP(insopt); + int index_ba= xiBa(insopt),num_ba= xnBa(insopt),index_clk= xiRb(insopt),num_clk= xnRb(insopt); + int index_dtrr= xiRd(insopt),num_dtrr= xnRd(insopt); // trace(3, "getPhi1:\n"); @@ -282,10 +327,10 @@ static void getPhi1(const int nx, double dt, const double *Cbe, } /* clk */ - if(nx>15){ - phi[index_clk+0+(index_dtrr+0)*nx]=dt; - phi[index_clk+1+(index_dtrr+1)*nx]=dt; - phi[index_clk+2+(index_dtrr+2)*nx]=dt; + if(insopt->tc==INSTC_SINGLE){ + for(i=0;igyro_noise_PSD ,dt,Q); - sysQ(3,3,nx,insopt->accel_noise_PSD,dt,Q); - sysQ(9,3,nx,insopt->accel_bias_PSD,dt,Q); - sysQ(12,3,nx,insopt->gyro_bias_PSD,dt,Q); - if(nx>15){ - sysQ(15,3,nx,insopt->clock_phase_PSD ,dt,Q); - sysQ(18,3,nx,insopt->clock_freq_PSD ,dt,Q); + sysQ(index_att,num_att,nx,insopt->gyro_noise_PSD ,dt,Q); + sysQ(index_vel,num_vel,nx,insopt->accel_noise_PSD,dt,Q); + sysQ(index_ba,num_ba,nx,insopt->accel_bias_PSD,dt,Q); + sysQ(index_bg,num_bg,nx,insopt->gyro_bias_PSD,dt,Q); + if(insopt->tc==INSTC_SINGLE){ + sysQ(index_clk,num_clk,nx,insopt->clock_phase_PSD ,dt,Q); + sysQ(index_dtrr,num_dtrr,nx,insopt->clock_freq_PSD ,dt,Q); } } static void setQ(const insopt_t *insopt,const double dt, const int nx, double *Q,const int upd){ @@ -350,7 +399,8 @@ static void propP(const insopt_t *insopt,const double *Q,const double *phi, } /* initialize every epoch for clock (white noise) */ // initP(15,3,nx,3000,UNC_CLK,P); - free(PQ); free(Phi2); + free(PQ); + free(Phi2); } /* propagate state estimates noting that all states are zero due to closed-loop * correction----------------------------------------------------------------*/ @@ -364,11 +414,11 @@ static void updstat(const insopt_t *insopt,ins_t *ins,const double dt, const double *x0,const double *P0,double *phi,double *P, double *x,double *Q,const int upd) { - int nx = ins->nx; + 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); + getPhi1(nx,dt,ins->Cbe,ins->position,ins->omgb,ins->fb,phi,insopt); getQ(insopt,dt,Q,nx); /* propagate state estimation error covariance */ @@ -395,7 +445,7 @@ static void updstat(const insopt_t *insopt,ins_t *ins,const double dt, extern void propinss(ins_t *ins,const insopt_t *insopt,double dt, double *x,double *P,const int upd) { - int nx = ins->nx; + int nx = xnX(insopt); double *phi,*Q; Q = mat(nx,nx); diff --git a/TC_NAV_Zjut_post/Src/rtkpos.c b/TC_NAV_Zjut_post/Src/rtkpos.c index 5b6ea92..f6c442b 100644 --- a/TC_NAV_Zjut_post/Src/rtkpos.c +++ b/TC_NAV_Zjut_post/Src/rtkpos.c @@ -450,20 +450,21 @@ static double baseline(const double *ru, const double *rb, double *dr) /* initialize state and covariance -------------------------------------------*/ static void initx(rtk_t *rtk, double xi, double var, int i) { - int j,tc; + int j,tc,nx; - tc = rtk->opt.insopt.tc; + tc = rtk->opt.mode; + nx = tc==PMODE_INS_TGNSS?rtk->ins.nx:rtk->nx; - if(tc==INSTC_RTK){ + if(tc==PMODE_INS_TGNSS){ rtk->ins.x[i]=xi; - for (j=0;jnx;j++) { - rtk->ins.P[i+j*rtk->ins.nx]=rtk->ins.P[j+i*rtk->ins.nx]=i==j?var:0.0; + for (j=0;jins.P[i+j*nx]=rtk->ins.P[j+i*nx]=i==j?var:0.0; } } else{ rtk->x[i]=xi; - for (j=0;jnx;j++) { - rtk->P[i+j*rtk->nx]=rtk->P[j+i*rtk->nx]=i==j?var:0.0; + for (j=0;jP[i+j*nx]=rtk->P[j+i*nx]=i==j?var:0.0; } } } @@ -533,7 +534,7 @@ static void udpos(rtk_t *rtk, double tt) ix=imat(rtk->nx,1); for (i=nx=0;inx;i++) { /* TODO: The b34 code causes issues so use b33 code for now */ - if (i<9||(rtk->x[i]!=0.0&&rtk->P[i+i*rtk->nx]>0.0)) ix[nx++]=i; + if (i<9||(rtk->x[i]!=0.0&&rtk->P[i+i*rtk->nx]>0.0)) ix[nx++]=i; // 这里保留前9维(pos,velocity,acc)+模糊度并记录他们在x中的index保存在ix中 } /* state transition of position/velocity/acceleration */ F=eye(nx); P=mat(nx,nx); FP=mat(nx,nx); x=mat(nx,1); xp=mat(nx,1); @@ -2251,7 +2252,7 @@ extern void init_tc(insopt_t *insopt, ins_t *ins, const prcopt_t *opt) int i; gtime_t t0={0}; trace(3,"initlc:\n"); - ins->nx = insopt->tc==INSTC_SINGLE?21:15+NB(opt); + ins->nx = xnX(insopt); ins->x =mat(ins->nx,1); ins->P =mat(ins->nx,ins->nx); ins->P0=zeros(ins->nx,ins->nx); @@ -2313,14 +2314,11 @@ extern void rtkinit(rtk_t *rtk, const prcopt_t *opt) rtk->opt=*opt; rtk->initial_mode=rtk->opt.mode; rtk->sol.thres=(float)opt->thresar[0]; + rtk->opt.insopt.gopt =&rtk->opt; /* ins */ if(opt->mode<=PMODE_INS_TGNSS){ init_tc(&rtk->opt.insopt,&rtk->ins,opt); -// switch(opt->insopt.tc){ -// case INSTC_SINGLE: rtk->opt.mode = PMODE_SINGLE; break; -// case INSTC_RTK: rtk->opt.mode = PMODE_KINEMA; break; -// } rtk->ins.rtkp=rtk; } else{ 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 0d3c772..6f4fb82 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: Feb 01 14:19 CST +Start testing: Feb 02 14:21 CST ---------------------------------------------------------- -End testing: Feb 01 14:19 CST +End testing: Feb 02 14:21 CST diff --git a/TC_NAV_Zjut_post/include/rtklib.h b/TC_NAV_Zjut_post/include/rtklib.h index 3f31f27..fd4d646 100644 --- a/TC_NAV_Zjut_post/include/rtklib.h +++ b/TC_NAV_Zjut_post/include/rtklib.h @@ -727,6 +727,7 @@ typedef struct { int tc; /* ins-gnss tightly coupled mode (INSTC_???) */ double L_ba_b[3]; /* lever arm from antenna to imu */ + void *gopt; /* pointer to option */ }insopt_t; @@ -1600,7 +1601,17 @@ EXPORT void rt_tcfilter(rtksvr_t *svr, int fobs, uint32_t tick); EXPORT int rt_init_ins(obsd_t *obs, rtksvr_t *svr, int n, imud_t *imud, int i_imu, int flag, const prcopt_t *prcopt); /* rt_ins-gnss ----------------------------------------------------------------------*/ +EXPORT int xnX(const insopt_t *insopt); +extern int xnB(const insopt_t *insopt); +EXPORT int xnP(const insopt_t *insopt); +EXPORT int xnV(const insopt_t *insopt); +EXPORT int xnA(const insopt_t *insopt); +EXPORT int xnBa(const insopt_t *insopt); +EXPORT int xnBg(const insopt_t *insopt); +EXPORT int xnRb(const insopt_t *insopt); +EXPORT int xnRd(const insopt_t *insopt); EXPORT int xiP(const insopt_t *insopt); +EXPORT int xnIF(const insopt_t *opt); EXPORT int xiV(const insopt_t *insopt); EXPORT int xiA(const insopt_t *insopt); EXPORT int xiBa(const insopt_t *insopt);