RTK-Visual-Inertial-Navigation
RTK-Visual-Inertial-Navigation copied to clipboard
xiaohong-huang
Satellite observation data
Hello, thank you for your excellent open-source project. May I ask how the following satellite observation data is obtained?
c
uint8_t RTK_LLI[NFREQ]; // RB-SD carrier-phase cycle slip flag uint8_t SPP_LLI[NFREQ]; // Rover-only carrier-phase cycle slip flag
Sorry, my view on LLI was wrong earlier. Here is our decode_rxmrawx function. It detailed how we obtained the SPP_LLI. And the RTK_LLI is obtained by RTK_LLI=SPP_LLI(rover)+SPP_LLI(base).
static int8_t decode_rxmrawx(raw_t* raw) { gtime_tt time; unsigned char* p = raw->buff + 6; uint16_t week; uint8_t n = 0, halfc, slip, code, sys, f, svid, pstd, dstd, tstat, cpstd, cn0, sigid, gnss, i, j, k, halfv; int16_t sat; int32_t lockt, nmeas; double P, L, D, tow; if (raw->len < 24) { return -1; } tow = R8(p); /* rcvTow (s) / week = U2(p + 8); / week / nmeas = U1(p + 11); / numMeas */
if (raw->len < 24 + 32 * nmeas) {
return -1;
}
if (week == 0) {
return 0;
}
time = gpst2time(week, tow);
// raw->time=time;
for (i = 0, p += 16; i < nmeas && n < MAXOBS; i++, p += 32) {
P = R8(p ); /* prMes (m) */
L = R8(p + 8); /* cpMes (cyc) */
D = R4(p + 16); /* doMes (hz) */
gnss = U1(p + 20); /* gnssId */
svid = U1(p + 21); /* svId */
sigid = U1(p + 22); /* sigId ([5] 5.15.3.1) */
lockt = U2(p + 24); /* locktime (ms) */
cn0 = U1(p + 26); /* cn0 (dBHz) */
cpstd = U1(p + 28) & 15; /* cpStdev (m) */
pstd = U1(p + 27);//0.01*2^nm
dstd = U1(p + 29);//0.002*2^nHZ
assert(pstd <= 15);
tstat = U1(p + 30); /* trkStat */
if (!(tstat & 1)) P = 0.0;
if (!(tstat & 2) || L == -0.5) L = 0.0;
sys = ubx_sys(gnss);
if (!sys) {
continue;
}
sat = satno(sys, svid);
if (sat == -1) {
std::cout << "no use sat:" << (int)sys << "," << (int)svid << std::endl;
// assert(0);
continue;
}
code = ubx_sig(sys, sigid);
/* signal index in obs data */
f = sig_idx(sys, code);
if (f == 0 || f > NFREQ) {
continue;
}
halfv = tstat & 4 ? 1 : 0; /* half cycle valid */
halfc = tstat & 8 ? 1 : 0; /* half cycle subtracted from phase */
slip = lockt == 0 || lockt * 1E-3 < raw->lockt[sat][f - 1]||
halfc != raw->halfc[sat][f - 1] || halfv != raw->halfv[sat][f - 1];
raw->lockt[sat][f - 1] = lockt * 1E-3;
raw->halfc[sat][f - 1] = halfc;
raw->halfv[sat][f - 1] = halfv;
raw->lastupdatetime[sat][f - 1] = time;
if (L == 0)memset(&raw->lastupdatetime[sat][f - 1], 0, sizeof(gtime_tt));
for (j = 0; j < n; j++) {
if (raw->obsData[j].sat == sat) break;
}
if (j >= n) {
raw->obsData[n].time = time;
raw->obsData[n].sat = (uint8_t)sat;
for (k = 0; k < NFREQ; k++) {
raw->obsData[n].SPP_L[k] = raw->obsData[n].SPP_P[k] = raw->obsData[n].SPP_D[k] = 0.0;
raw->obsData[n].SNR[k] = raw->obsData[n].SPP_LLI[k] = raw->obsData[n].half_flag[k] = raw->obsData[n].SPP_Lstd[k] = 0;
}
n++;
}
if (slip) {
raw->slip[sat][f - 1]++;
}
raw->obsData[j].SPP_L[f - 1] = L;
raw->obsData[j].SPP_P[f - 1] = P;
raw->obsData[j].SPP_D[f - 1] = D;
raw->obsData[j].SNR[f - 1] = (uint8_t)(cn0 * 4);
raw->obsData[j].SPP_LLI[f - 1] = raw->slip[sat][f - 1];
raw->obsData[j].half_flag[f - 1] = halfv << 1 | halfc;
raw->obsData[j].SPP_Pstd[f - 1] = pow(2, pstd) * 0.01;
raw->obsData[j].SPP_Lstd[f - 1] = 0.004 * cpstd;
raw->obsData[j].SPP_Dstd[f - 1] = 0.002 * pow(2, dstd);
}
raw->obsN = n;
return 1;
}
Sorry, my view on LLI was wrong earlier. Here is our decode_rxmrawx function. It detailed how we obtained the SPP_LLI. And the RTK_LLI is obtained by RTK_LLI=SPP_LLI(rover)+SPP_LLI(base).
static int8_t decode_rxmrawx(raw_t* raw) { gtime_tt time; unsigned char* p = raw->buff + 6; uint16_t week; uint8_t n = 0, halfc, slip, code, sys, f, svid, pstd, dstd, tstat, cpstd, cn0, sigid, gnss, i, j, k, halfv; int16_t sat; int32_t lockt, nmeas; double P, L, D, tow; if (raw->len < 24) { return -1; } tow = R8(p); /* rcvTow (s) / week = U2(p + 8); / week / nmeas = U1(p + 11); / numMeas */
if (raw->len < 24 + 32 * nmeas) { return -1; } if (week == 0) { return 0; } time = gpst2time(week, tow); // raw->time=time; for (i = 0, p += 16; i < nmeas && n < MAXOBS; i++, p += 32) { P = R8(p ); /* prMes (m) */ L = R8(p + 8); /* cpMes (cyc) */ D = R4(p + 16); /* doMes (hz) */ gnss = U1(p + 20); /* gnssId */ svid = U1(p + 21); /* svId */ sigid = U1(p + 22); /* sigId ([5] 5.15.3.1) */ lockt = U2(p + 24); /* locktime (ms) */ cn0 = U1(p + 26); /* cn0 (dBHz) */ cpstd = U1(p + 28) & 15; /* cpStdev (m) */ pstd = U1(p + 27);//0.01*2^nm dstd = U1(p + 29);//0.002*2^nHZ assert(pstd <= 15); tstat = U1(p + 30); /* trkStat */ if (!(tstat & 1)) P = 0.0; if (!(tstat & 2) || L == -0.5) L = 0.0; sys = ubx_sys(gnss); if (!sys) { continue; } sat = satno(sys, svid); if (sat == -1) { std::cout << "no use sat:" << (int)sys << "," << (int)svid << std::endl; // assert(0); continue; } code = ubx_sig(sys, sigid); /* signal index in obs data */ f = sig_idx(sys, code); if (f == 0 || f > NFREQ) { continue; } halfv = tstat & 4 ? 1 : 0; /* half cycle valid */ halfc = tstat & 8 ? 1 : 0; /* half cycle subtracted from phase */ slip = lockt == 0 || lockt * 1E-3 < raw->lockt[sat][f - 1]|| halfc != raw->halfc[sat][f - 1] || halfv != raw->halfv[sat][f - 1]; raw->lockt[sat][f - 1] = lockt * 1E-3; raw->halfc[sat][f - 1] = halfc; raw->halfv[sat][f - 1] = halfv; raw->lastupdatetime[sat][f - 1] = time; if (L == 0)memset(&raw->lastupdatetime[sat][f - 1], 0, sizeof(gtime_tt)); for (j = 0; j < n; j++) { if (raw->obsData[j].sat == sat) break; } if (j >= n) { raw->obsData[n].time = time; raw->obsData[n].sat = (uint8_t)sat; for (k = 0; k < NFREQ; k++) { raw->obsData[n].SPP_L[k] = raw->obsData[n].SPP_P[k] = raw->obsData[n].SPP_D[k] = 0.0; raw->obsData[n].SNR[k] = raw->obsData[n].SPP_LLI[k] = raw->obsData[n].half_flag[k] = raw->obsData[n].SPP_Lstd[k] = 0; } n++; } if (slip) { raw->slip[sat][f - 1]++; } raw->obsData[j].SPP_L[f - 1] = L; raw->obsData[j].SPP_P[f - 1] = P; raw->obsData[j].SPP_D[f - 1] = D; raw->obsData[j].SNR[f - 1] = (uint8_t)(cn0 * 4); raw->obsData[j].SPP_LLI[f - 1] = raw->slip[sat][f - 1]; raw->obsData[j].half_flag[f - 1] = halfv << 1 | halfc; raw->obsData[j].SPP_Pstd[f - 1] = pow(2, pstd) * 0.01; raw->obsData[j].SPP_Lstd[f - 1] = 0.004 * cpstd; raw->obsData[j].SPP_Dstd[f - 1] = 0.002 * pow(2, dstd); } raw->obsN = n; return 1;}
Thank you for your code. I have a new question. Based on your GNSS factor code, RTK_P and RTK_L seem to be the corrected measurements of the rover. Could you please explain the correction process for RTK_P and RTK_L?
`bool RTKCarrierPhaseFactor::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { const double* xyz = parameters[0]; const double* PB1 = parameters[1]; const double* dtur = parameters[2];
double r1, e1[3];
double xyzglobal[3];
xyzglobal[0] = xyz[0] + base_pos[0];
xyzglobal[1] = xyz[1] + base_pos[1];
xyzglobal[2] = xyz[2] + base_pos[2];
r1 = distance(xyzglobal, satelite_pos, e1);
double sqrt_info = 1;
if (use_istd)sqrt_info = 1 / sqrt(varerr2(el, base_rover_time_diff, mea_var));
if (residuals)
residuals[0] = sqrt_info * (r1 - *PB1 * lam - L1_lam + *dtur);//PB1为整周模糊度，L1_lam为载波测量值（包括整周数），dtur为基站和流动站的时间误差和光速的乘积
if (jacobians) {
if (jacobians[0]) {
memset(jacobians[0], 0, sizeof(double) * 7);
jacobians[0][0] = sqrt_info * e1[0];
jacobians[0][1] = sqrt_info * e1[1];
jacobians[0][2] = sqrt_info * e1[2];
}
if (jacobians[1]) {
jacobians[1][0] = -sqrt_info * lam;
}
if (jacobians[2]) {
jacobians[2][0] = sqrt_info;
}
}
return true;
}`
Here is a simple correction demo: RTK_P=SPP_P(rover)-(SPP_P(base)-db), RTK_L=SPP_L(rover)-(SPP_L(base)-db/wave_length),
with db=geometry range between satellite and base station.
Here is a simple correction demo: RTK_P=SPP_P(rover)-(SPP_P(base)-db), RTK_L=SPP_L(rover)-(SPP_L(base)-db/wave_length),
with db=geometry range between satellite and base station.
I understand, thank you for your reply!
Sorry to bother you again. We have encountered a new issue. We checked the u-blox F9p user manual and found that the u-blox F9p receiver does not output the parameters sat_var, ion_var, and trop_var. Could you please tell us how you obtain these parameters?
typedef struct { ..... double sat_var; //satellite noise covariance [m^2] double ion_var; //iono noise covariance [m^2] double trop_var; //trop noise covariance [m^2] ..... } ObsMea;
function rescode(): vare[i] : sat_var vion : ion_var vtrp : trop_var
Such a solid work. I didn't see any ionospheric or tropospheric corrections on SppPseudorangeFactor ? Does it correctted before input or somewhere else
The ionospheric and tropospheric corrections were already implemented during our hardware data collection, so yes, the following variables were corrected before input.
double SPP_P[NFREQ];//Rover-only pseudorange measurement(with correction) [m] double SPP_L[NFREQ];//Rover-only carrier-phase measurement(with correction) [cycle] double SPP_D[NFREQ];//Rover-only doppler measurement(with correction) [cycle/s]
Thank you for your prompt reply. The ionospheric and tropospheric corrections require the position information of the receiver. If the receiver's position is unknown prior to input, how can the correction be made? Is it by using SPP to roughly estimate the rover position, or by directly utilizing the base position?
Yes, the initial rover position is estimated by SPP, which is good enough for correction.
Hi, I'm new to GVINS, may I ask you how to get satellite number from satellite observation data? And I also don't know where is the satellite observation data? Is it the bag file? @xiaohong-huang
According to the code you provided above,
halfv = tstat & 4 ? 1 : 0; /* half cycle valid */
halfc = tstat & 8 ? 1 : 0; /* half cycle subtracted from phase */
raw->obsData[j].half_flag[f - 1] = halfv << 1 | halfc;
only lower 2 bit (bit0, bit1) can be set to half_flag. but you check the bit3 and bit1 in UpdateNParameterHead and LambdaSearch functions.
Did I misunderstand something?
Dear Xiaohong Huang, the information provided here helped to convert and pre-process our proprietary data. However, the performance with our data is not always as good in "open-sky conditions" as reported in the paper. We are unsure if we did the conversion and pre-processing correctly. Is there a more specific guideline? Our impression is that the GNSS processing is very similar to that of RTKLIB. Was RTKLIB used for pre-processing? If yes, would it be possible to indicate which intermediate results were logged?
We are working with RTKPseudorangeFactor and RTKCarrierPhaseFactor to be "independent" of the atmospheric corrections.
The preprocessing of differential observations is very simple and can be handled with RTKLIB(check the zdres function), and errors in this part are unlikely—look elsewhere for the issue. Or just share your results, and I’ll take a look to see what might have gone wrong.
Thanks for your advice! I will have a closer look at the zdres function of RTKLIB.
I expect that the error is elsewhere as well. Our concerns are more:
- How to compute the single difference range and phase observations for rover epochs between two reference epochs. (We do not have the same rate for the reference epochs and tried different interpolations.)
- We observed that movement is necessary for initialization. If there is a static phase at the beginning, the attitude will not converge. (I attached two error plots of a dataset with static phase at the beginning. In one case, attitude does not converge, in the other case it does by shifting the starting point by 5s.)
- We were unable to extract any meaningful covariances from the ceres problem to better understand the behavior. (Though, we have to spend more time on this.)
First, as mentioned in my paper, it’s sufficient to pair a rover with any base station whose observation epoch differs by less than one second; precise time alignment is unnecessary. This strategy is standard in RTK processing.
Unfortunately, I cannot deduce from the data you provided why your results are so poor. I recommend running your dataset through RTKLIB first. In open-sky conditions, RTKLIB should achieve an almost 100 % fix rate. Once you can reproduce that performance in RTKLIB, you’ll have a solid grasp of RTK fundamentals, and it will then be straightforward to infer the preprocessing steps that I haven’t released.