xiaohong-huang / RTK-Visual-Inertial-Navigation

A method used sliding window filter frame work for RTK-Visual-Inertial-Navigation
GNU General Public License v3.0
85 stars 12 forks source link

Satellite observation data #3

Open 201907030402 opened 2 weeks ago

201907030402 commented 2 weeks ago

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

201907030402 commented 2 weeks ago

They are obtained by decoding the ubx-rxm-rawx

I understand, thank you very much for your response!

xiaohong-huang commented 2 weeks ago

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;

}

201907030402 commented 1 week ago

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; int32t 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;

}`

xiaohong-huang commented 1 week ago

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.

201907030402 commented 1 week ago

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!

201907030402 commented 3 days ago

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;

xiaohong-huang commented 3 days ago

See here.

function rescode(): vare[i] : sat_var vion : ion_var vtrp : trop_var

201907030402 commented 3 days ago

I understand, thank you for your reply!