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

bug fix

Browse Source
This commit is contained in:
Zhiqiang_Yang98 2023-04-13 15:45:42 +08:00
parent 176a4b232e
commit d86f132926
2 changed files with 14 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
TC_NAV_Zjut_post/Src/pntpos.c
View File

@ -657,7 +657,9 @@ static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
trace(3,"estpos : n=%d\n",n); trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1); v=mat(n+4,1);
H=mat(NX,n+4);
var=mat(n+4,1);
for (i=0;i<3;i++) x[i]=sol->rr[i]; for (i=0;i<3;i++) x[i]=sol->rr[i];
@ -797,15 +799,15 @@ static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
{ {
double freq,rate,pos[3],E[9],a[3],e[3],vs[3],cosel,sig; double freq,rate,pos[3],E[9],a[3],e[3],vs[3],cosel,sig;
int i,j,nv=0; int i,j,nv=0;
trace(3,"resdop : n=%d\n",n); trace(3,"resdop : n=%d\n",n);
ecef2pos(rr,pos); xyz2enu(pos,E); ecef2pos(rr,pos); xyz2enu(pos,E);
for (i=0;i<n&&i<MAXOBS;i++) { for (i=0;i<n&&i<MAXOBS;i++) {
freq=sat2freq(obs[i].sat,obs[i].code[0],nav); freq=sat2freq(obs[i].sat,obs[i].code[0],nav);
if (obs[i].D[0]==0.0||freq==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) { if (obs[i].D[0]==0.0||freq==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) {
continue; continue;
} }
@ -815,7 +817,7 @@ static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
a[1]=cos(azel[i*2])*cosel; a[1]=cos(azel[i*2])*cosel;
a[2]=sin(azel[1+i*2]); a[2]=sin(azel[1+i*2]);
matmul("TN",3,1,3,1.0,E,a,0.0,e); matmul("TN",3,1,3,1.0,E,a,0.0,e);
/* satellite velocity relative to receiver in ECEF */ /* satellite velocity relative to receiver in ECEF */
for (j=0;j<3;j++) { for (j=0;j<3;j++) {
vs[j]=rs[j+3+i*6]-x[j]; vs[j]=rs[j+3+i*6]-x[j];
@ -823,17 +825,13 @@ static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
/* range rate with earth rotation correction */ /* range rate with earth rotation correction */
rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]- rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]-
rs[3+i*6]*rr[1]-rs[ i*6]*x[1]); rs[3+i*6]*rr[1]-rs[ i*6]*x[1]);
/* Std of range rate error (m/s) */ /* Std of range rate error (m/s) */
sig=(err<=0.0)?1.0:err*CLIGHT/freq; sig=(err<=0.0)?1.0:err*CLIGHT/freq;
/* range rate residual (m/s) */ /* range rate residual (m/s) */
#ifdef LG69T v[nv]=(obs[i].D[0]*CLIGHT/freq-(rate+x[3]-CLIGHT*dts[1+i*2]))/sig;
v[2*nv+1]=(-obs[i].D[0]*CLIGHT/freq-(rate-CLIGHT*dts[1+i*2]));
#else
v[2*nv+1]=(obs[i].D[0]*CLIGHT/freq-(rate-CLIGHT*dts[1+i*2]));
#endif
/* design matrix */ /* design matrix */
for (j=0;j<4;j++) { for (j=0;j<4;j++) {
H[j+nv*4]=((j<3)?-e[j]:1.0)/sig; H[j+nv*4]=((j<3)?-e[j]:1.0)/sig;

2
TC_NAV_Zjut_post/config/Tc.conf
View File

@ -134,7 +134,7 @@ inpstr3-type =serial # (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:n
inpstr1-path =ttyACM2:460800:8:n:1:off inpstr1-path =ttyACM2:460800:8:n:1:off
# inpstr2-path =caxm002:m2ywj94n@sdk.pnt.10086.cn:8002/RTCM33_GRCEJ # inpstr2-path =caxm002:m2ywj94n@sdk.pnt.10086.cn:8002/RTCM33_GRCEJ
inpstr2-path = inpstr2-path =
inpstr3-path =ttyACM0:921600:8:n:1:off inpstr3-path =ttyACM3:921600:8:n:1:off
inpstr1-format =rtcm3 # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu) inpstr1-format =rtcm3 # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu)
inpstr2-format =rtcm3 # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu) inpstr2-format =rtcm3 # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu)
inpstr3-format =imu # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu) inpstr3-format =imu # (0:rtcm2,1:rtcm3,2:oem4,4:ubx,5:swift,6:hemis,7:skytraq,8:javad,9:nvs,10:binex,11:rt17,12:sbf,15:sp3,19:imu)