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

规范程序变量命名，针对rtk/ins与spp/ins情况做出却别。

Browse Source
This commit is contained in:
ZhiQiang_Yang98 2023-02-03 14:58:51 +08:00
parent 82435740dc
commit 2d3d98f091
4 changed files with 98 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -51,17 +51,59 @@ extern void rt_insopt_init(insopt_t *insopt){
#endif #endif
} }
/* get number of phase bias */
/* index if position */ 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;} extern int xiP(const insopt_t *insopt) {return 6;}
/* index if velocity */ /* index of velocity */
extern int xiV(const insopt_t *insopt) {return 3;} extern int xiV(const insopt_t *insopt) {return 3;}
/* index if attitude */ /* index of attitude */
extern int xiA(const insopt_t *insopt) {return 0;} 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;} 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;} 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--------------------------------------------*/ /* system noise covariance matrix--------------------------------------------*/
static void sysQ(int is,int n,int nx,double v,double dt,double *Q) 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)---------------*/ /* determine transition matrix(first-order approx: Phi=I+F*dt)---------------*/
static void getPhi1(const int nx, double dt, const double *Cbe, static void getPhi1(const int nx, double dt, const double *Cbe,
const double *pos, const double *omgb, const double *fib, const double *pos, const double *omgb, const double *fib,
double *phi) { double *phi,const insopt_t *insopt)
{
int i, j; int i, j;
double omega[3] = {0}, T[9], ge[3], re, rn[3], Cbv[9]; double omega[3] = {0}, T[9], ge[3], re, rn[3], Cbv[9];
double W2[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_att = xiA(insopt),num_att=xnA(insopt),index_bg=xiBg(insopt),num_bg=xnBg(insopt);
int index_ba=9,num_ba=3,index_clk=15,num_clk=3,index_dtrr=18; 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"); // trace(3, "getPhi1:\n");
@ -282,10 +327,10 @@ static void getPhi1(const int nx, double dt, const double *Cbe,
} }
/* clk */ /* clk */
if(nx>15){ if(insopt->tc==INSTC_SINGLE){
phi[index_clk+0+(index_dtrr+0)*nx]=dt; for(i=0;i<num_dtrr;i++){
phi[index_clk+1+(index_dtrr+1)*nx]=dt; phi[index_clk+i+(index_dtrr+i)*nx]=dt;
phi[index_clk+2+(index_dtrr+2)*nx]=dt; }
} }
stochasticPhi(index_ba,num_ba,nx,dt,phi); stochasticPhi(index_ba,num_ba,nx,dt,phi);
@ -300,14 +345,18 @@ static void getQ(const insopt_t *insopt,double dt,double* Q,const int nx)
trace(3,"too large time difference of ins and gnss measurements\n"); trace(3,"too large time difference of ins and gnss measurements\n");
} }
setzero(Q,nx,nx); setzero(Q,nx,nx);
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);
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);
sysQ(0,3,nx,insopt->gyro_noise_PSD ,dt,Q); sysQ(index_att,num_att,nx,insopt->gyro_noise_PSD ,dt,Q);
sysQ(3,3,nx,insopt->accel_noise_PSD,dt,Q); sysQ(index_vel,num_vel,nx,insopt->accel_noise_PSD,dt,Q);
sysQ(9,3,nx,insopt->accel_bias_PSD,dt,Q); sysQ(index_ba,num_ba,nx,insopt->accel_bias_PSD,dt,Q);
sysQ(12,3,nx,insopt->gyro_bias_PSD,dt,Q); sysQ(index_bg,num_bg,nx,insopt->gyro_bias_PSD,dt,Q);
if(nx>15){ if(insopt->tc==INSTC_SINGLE){
sysQ(15,3,nx,insopt->clock_phase_PSD ,dt,Q); sysQ(index_clk,num_clk,nx,insopt->clock_phase_PSD ,dt,Q);
sysQ(18,3,nx,insopt->clock_freq_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){ 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) */ /* initialize every epoch for clock (white noise) */
// initP(15,3,nx,3000,UNC_CLK,P); // 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 /* propagate state estimates noting that all states are zero due to closed-loop
* correction----------------------------------------------------------------*/ * 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, const double *x0,const double *P0,double *phi,double *P,
double *x,double *Q,const int upd) 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); // setphi(ins->Cbe,ins->fb,ins->position,dt,phi,nx);
// setQ(insopt,dt,nx,Q,upd); // 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); getQ(insopt,dt,Q,nx);
/* propagate state estimation error covariance */ /* 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, extern void propinss(ins_t *ins,const insopt_t *insopt,double dt,
double *x,double *P,const int upd) double *x,double *P,const int upd)
{ {
int nx = ins->nx; int nx = xnX(insopt);
double *phi,*Q; double *phi,*Q;
Q = mat(nx,nx); Q = mat(nx,nx);

24
TC_NAV_Zjut_post/Src/rtkpos.c
View File

@ -450,20 +450,21 @@ static double baseline(const double *ru, const double *rb, double *dr)
/* initialize state and covariance -------------------------------------------*/ /* initialize state and covariance -------------------------------------------*/
static void initx(rtk_t *rtk, double xi, double var, int i) 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; rtk->ins.x[i]=xi;
for (j=0;j<rtk->nx;j++) { for (j=0;j<nx;j++) {
rtk->ins.P[i+j*rtk->ins.nx]=rtk->ins.P[j+i*rtk->ins.nx]=i==j?var:0.0; rtk->ins.P[i+j*nx]=rtk->ins.P[j+i*nx]=i==j?var:0.0;
} }
} }
else{ else{
rtk->x[i]=xi; rtk->x[i]=xi;
for (j=0;j<rtk->nx;j++) { for (j=0;j<nx;j++) {
rtk->P[i+j*rtk->nx]=rtk->P[j+i*rtk->nx]=i==j?var:0.0; rtk->P[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); ix=imat(rtk->nx,1);
for (i=nx=0;i<rtk->nx;i++) { for (i=nx=0;i<rtk->nx;i++) {
/* TODO: The b34 code causes issues so use b33 code for now */ /* 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 */ /* 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); 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}; int i; gtime_t t0={0};
trace(3,"initlc:\n"); 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->x =mat(ins->nx,1);
ins->P =mat(ins->nx,ins->nx); ins->P =mat(ins->nx,ins->nx);
ins->P0=zeros(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->opt=*opt;
rtk->initial_mode=rtk->opt.mode; rtk->initial_mode=rtk->opt.mode;
rtk->sol.thres=(float)opt->thresar[0]; rtk->sol.thres=(float)opt->thresar[0];
rtk->opt.insopt.gopt =&rtk->opt;
/* ins */ /* ins */
if(opt->mode<=PMODE_INS_TGNSS){ if(opt->mode<=PMODE_INS_TGNSS){
init_tc(&rtk->opt.insopt,&rtk->ins,opt); 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; rtk->ins.rtkp=rtk;
} }
else{ else{

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

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

11
TC_NAV_Zjut_post/include/rtklib.h
View File

@ -727,6 +727,7 @@ typedef struct {
int tc; /* ins-gnss tightly coupled mode (INSTC_???) */ int tc; /* ins-gnss tightly coupled mode (INSTC_???) */
double L_ba_b[3]; /* lever arm from antenna to imu */ double L_ba_b[3]; /* lever arm from antenna to imu */
void *gopt; /* pointer to option */
}insopt_t; }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, 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); const prcopt_t *prcopt);
/* rt_ins-gnss ----------------------------------------------------------------------*/ /* 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 xiP(const insopt_t *insopt);
EXPORT int xnIF(const insopt_t *opt);
EXPORT int xiV(const insopt_t *insopt); EXPORT int xiV(const insopt_t *insopt);
EXPORT int xiA(const insopt_t *insopt); EXPORT int xiA(const insopt_t *insopt);
EXPORT int xiBa(const insopt_t *insopt); EXPORT int xiBa(const insopt_t *insopt);