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

更新nhc

Browse Source
This commit is contained in:
ZhiQiang_Yang98 2022-11-28 16:19:13 +08:00
parent 15798d1b79
commit 8334bb29cc
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
TC_NAV_Zjut_post/Src/post/ps_tcpos.c
View File

@ -474,7 +474,7 @@ static int tcfilter()
outsolstat(&svr.rtk,&nav); outsolstat(&svr.rtk,&nav);
} }
/* non-holonomic constraint */ /* non-holonomic constraint */
// nhc(&svr.rtk.ins,&svr.rtk.opt.insopt,&imud); nhc(&svr.rtk.ins,&svr.rtk.opt.insopt,&imud);
// //
// /* zero velocity/zero angular rate update */ // /* zero velocity/zero angular rate update */
// ecef2pos(svr.rtk.ins.position,pos); // ecef2pos(svr.rtk.ins.position,pos);

8
TC_NAV_Zjut_post/Src/real_time/nhc.c
View File

@ -4,7 +4,7 @@
#include "rtklib.h" #include "rtklib.h"
#define MAXVEL 0.5 /* max velocity for using non-holonomic constraint */ #define MAXVEL 0.5 /* max velocity for using non-holonomic constraint */
#define VARVEL SQR(0.3) /* initial variance of ins velocity ((m/s)^2) */ #define VARVEL SQR(0.05) /* initial variance of ins velocity ((m/s)^2) */
/* measurement sensitive-matrix for non-holonomic----------------------------*/ /* measurement sensitive-matrix for non-holonomic----------------------------*/
static int bldnhc(const insopt_t *opt,const imud_t *imu,const double *Cbe, static int bldnhc(const insopt_t *opt,const imud_t *imu,const double *Cbe,
@ -44,7 +44,7 @@ static int bldnhc(const insopt_t *opt,const imud_t *imu,const double *Cbe,
trace(2,"too large vehicle turn\n"); trace(2,"too large vehicle turn\n");
continue; continue;
} }
H[IA+nv*nx]=T[i]; H[IA+1+nv*nx]=T[i+3]; H[IA+2+nv*nx]=T[i+6]; H[IA+nv*nx]=-T[i]; H[IA+1+nv*nx]=-T[i+3]; H[IA+2+nv*nx]=-T[i+6];
H[IV+nv*nx]=C[i]; H[IV+1+nv*nx]=C[i+3]; H[IV+2+nv*nx]=C[i+6]; H[IV+nv*nx]=C[i]; H[IV+1+nv*nx]=C[i+3]; H[IV+2+nv*nx]=C[i+6];
v[nv ]=vb[i]; v[nv ]=vb[i];
@ -67,8 +67,8 @@ extern int nhc(ins_t *ins,const insopt_t *opt,const imud_t *imu)
trace(3,"nhc:\n"); trace(3,"nhc:\n");
H=zeros(2,nx); R=zeros(2,2); H=zeros(nx,2); R=zeros(2,2);
v=zeros(2,1); x=zeros(1,nx); v=zeros(2,1); x=zeros(nx,1);
nv=bldnhc(opt,imu,ins->Cbe,ins->velocity,nx,v,H,R); nv=bldnhc(opt,imu,ins->Cbe,ins->velocity,nx,v,H,R);