mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-10-13 19:21:50 +00:00
Code Issues Releases Wiki Activity

Improve robustness against a corrupted reference satellite

Browse Source
This commit is contained in:
Carles Fernandez
2026-08-22 11:10:08 +02:00
parent cb7366d2d9
commit ac3f3f8e4c
2 changed files with 216 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
+126 -38
View File
@@ -1736,7 +1736,6 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
double fi;
double fj;
double df;
double threshadj;
double *Hi = nullptr;
int i;
int j;
@@ -1788,22 +1787,109 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
tropr[i] = prectrop(rtk->sol.time, posr, 1, azel + ir[i] * 2, opt, x, dtdxr + i * 3);
}
}
/* Calculate the same state-corrected double difference used by the
measurement loop. Reference selection uses it to find the mutually
consistent satellite cluster before choosing its lowest-variance pivot. */
const auto measurement_residual = [&](int reference, int target, int measurement) {
const int frequency = measurement % nf;
const int reference_satellite = sat[reference];
const int target_satellite = sat[target];
const int reference_system = rtk->ssat[reference_satellite - 1].sys;
const int target_system = rtk->ssat[target_satellite - 1].sys;
const double reference_wavelength = nav->lam[reference_satellite - 1][frequency];
const double target_wavelength = nav->lam[target_satellite - 1][frequency];
double residual =
(y[measurement + iu[reference] * nf * 2] - y[measurement + ir[reference] * nf * 2]) -
(y[measurement + iu[target] * nf * 2] - y[measurement + ir[target] * nf * 2]);
if (opt->ionoopt == IONOOPT_EST)
{
const double reference_frequency_ratio = reference_wavelength / LAM_CARR[0];
const double target_frequency_ratio = target_wavelength / LAM_CARR[0];
const double reference_factor =
(measurement < nf ? -1.0 : 1.0) *
reference_frequency_ratio * reference_frequency_ratio * im[reference];
const double target_factor =
(measurement < nf ? -1.0 : 1.0) *
target_frequency_ratio * target_frequency_ratio * im[target];
residual -= reference_factor * x[II_RTK(reference_satellite, opt)] -
target_factor * x[II_RTK(target_satellite, opt)];
}
if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG)
{
residual -= (tropu[reference] - tropu[target]) -
(tropr[reference] - tropr[target]);
}
if (measurement < nf)
{
if (opt->ionoopt != IONOOPT_IFLC)
{
residual -= reference_wavelength * x[IB_RTK(reference_satellite, measurement, opt)] -
target_wavelength * x[IB_RTK(target_satellite, measurement, opt)];
}
else
{
residual -= x[IB_RTK(reference_satellite, measurement, opt)] -
x[IB_RTK(target_satellite, measurement, opt)];
}
}
if (opt->glomodear == 2 && reference_system == SYS_GLO && target_system == SYS_GLO &&
frequency < NFREQGLO)
{
const double frequency_difference =
(SPEED_OF_LIGHT_M_S / reference_wavelength - SPEED_OF_LIGHT_M_S / target_wavelength) / 1E6;
residual -= frequency_difference * x[IL_RTK(frequency, opt)];
}
else if (reference_system == SYS_GLO && target_system == SYS_GLO)
{
residual -= gloicbcorr(reference_satellite, target_satellite, opt,
reference_wavelength, target_wavelength, measurement);
}
return residual;
};
/* Match the measurement gate exactly, including the temporary opening
used while either phase bias has just been initialized. */
const auto innovation_threshold = [&](int reference, int target, int measurement) {
double threshold = opt->maxinno[measurement < nf ? 0 : 1];
if (threshold <= 0.0 || P == nullptr || opt->mode <= PMODE_DGPS)
{
return threshold;
}
const int frequency = measurement % nf;
const int reference_bias = IB_RTK(sat[reference], frequency, opt);
const int target_bias = IB_RTK(sat[target], frequency, opt);
const double initial_variance = std::pow(opt->std[0], 2.0);
if (P[reference_bias + rtk->nx * reference_bias] == initial_variance ||
P[target_bias + rtk->nx * target_bias] == initial_variance)
{
threshold *= INNOVATION_THRESHOLD_INIT_FACTOR;
}
return threshold;
};
const auto innovation_outlier = [&](int reference, int target, int measurement, double residual) {
const double threshold = innovation_threshold(reference, target, measurement);
return threshold > 0.0 && fabs(residual) > threshold;
};
for (m = 0; m < 5; m++)
{ /* m=0:gps/sbs, 1:glo, 2:gal, 3:bds, 4:qzs */
for (f = opt->mode > PMODE_DGPS ? 0 : nf; f < nf * 2; f++)
{
frq = f % nf;
/* choose the minimum-variance satellite as the reference
(demo5: better than highest elevation in urban
conditions, where SNR is the better quality proxy),
preferring one without a slip and with a valid lock;
one pass tracks the best clean and the best overall
candidate, and the overall one is used only if no
clean reference exists */
/* Choose a reference from the largest innovation-consistent
satellite cluster, breaking ties by measurement variance.
This keeps a high-SNR outlier from becoming the pivot and
making every healthy satellite appear faulty. As before,
prefer a reference without a slip and with a valid lock. */
minvar = 0.0;
double minvar_any = 0.0;
int ref_any = -1;
int max_support = -1;
int max_support_any = -1;
for (i = -1, j = 0; j < ns; j++)
{
sysj = rtk->ssat[sat[j] - 1].sys;
@@ -1815,23 +1901,51 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
{
continue;
}
if (nav->lam[sat[j] - 1][frq] <= 0.0)
{
continue;
}
int support = 0;
for (k = 0; k < ns; k++)
{
if (k == j)
{
continue;
}
const int peer_system = rtk->ssat[sat[k] - 1].sys;
if (!test_sys(peer_system, m) ||
!validobs(iu[k], ir[k], f, nf, y) ||
nav->lam[sat[k] - 1][frq] <= 0.0)
{
continue;
}
if (!innovation_outlier(j, k, f,
measurement_residual(j, k, f)))
{
support++;
}
}
refvar = varerr(sat[j], sysj, azel[1 + iu[j] * 2],
rtk->ssat[sat[j] - 1].snr_rover[frq], rtk->ssat[sat[j] - 1].snr_base[frq],
bl, dt, f, opt, &obs[iu[j]]);
if (ref_any < 0 || refvar < minvar_any)
if (ref_any < 0 || support > max_support_any ||
(support == max_support_any && refvar < minvar_any))
{
ref_any = j;
minvar_any = refvar;
max_support_any = support;
}
if ((rtk->ssat[sat[j] - 1].slip[frq] & 1) ||
rtk->ssat[sat[j] - 1].lock[frq] < 0)
{
continue;
}
if (i < 0 || refvar < minvar)
if (i < 0 || support > max_support ||
(support == max_support && refvar < minvar))
{
i = j;
minvar = refvar;
max_support = support;
}
}
if (i < 0)
@@ -1878,8 +1992,7 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
}
}
/* double-differenced residual */
v[nv] = (y[f + iu[i] * nf * 2] - y[f + ir[i] * nf * 2]) -
(y[f + iu[j] * nf * 2] - y[f + ir[j] * nf * 2]);
v[nv] = measurement_residual(i, j, f);
/* partial derivatives by rover position */
if (H)
@@ -1896,7 +2009,6 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
fj = lamj / LAM_CARR[0];
didxi = (f < nf ? -1.0 : 1.0) * fi * fi * im[i];
didxj = (f < nf ? -1.0 : 1.0) * fj * fj * im[j];
v[nv] -= didxi * x[II_RTK(sat[i], opt)] - didxj * x[II_RTK(sat[j], opt)];
if (H)
{
Hi[II_RTK(sat[i], opt)] = didxi;
@@ -1906,7 +2018,6 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
/* double-differenced tropospheric delay term */
if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG)
{
v[nv] -= (tropu[i] - tropu[j]) - (tropr[i] - tropr[j]);
for (k = 0; k < (opt->tropopt < TROPOPT_ESTG ? 1 : 3); k++)
{
if (!H)
@@ -1922,7 +2033,6 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
{
if (opt->ionoopt != IONOOPT_IFLC)
{
v[nv] -= lami * x[IB_RTK(sat[i], f, opt)] - lamj * x[IB_RTK(sat[j], f, opt)];
if (H)
{
Hi[IB_RTK(sat[i], f, opt)] = lami;
@@ -1931,7 +2041,6 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
}
else
{
v[nv] -= x[IB_RTK(sat[i], f, opt)] - x[IB_RTK(sat[j], f, opt)];
if (H)
{
Hi[IB_RTK(sat[i], f, opt)] = 1.0;
@@ -1943,17 +2052,11 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
if (rtk->opt.glomodear == 2 && sysi == SYS_GLO && sysj == SYS_GLO && ff < NFREQGLO)
{
df = (SPEED_OF_LIGHT_M_S / lami - SPEED_OF_LIGHT_M_S / lamj) / 1E6; /* freq-difference (MHz) */
v[nv] -= df * x[IL_RTK(ff, opt)];
if (H)
{
Hi[IL_RTK(ff, opt)] = df;
}
}
/* glonass interchannel bias correction */
else if (sysi == SYS_GLO && sysj == SYS_GLO)
{
v[nv] -= gloicbcorr(sat[i], sat[j], &rtk->opt, lami, lamj, f);
}
if (f < nf)
{
rtk->ssat[sat[j] - 1].resc[f] = v[nv];
@@ -1963,23 +2066,8 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
rtk->ssat[sat[j] - 1].resp[f - nf] = v[nv];
}
/* open up the innovation threshold if one of the two phase
biases has just been initialized, otherwise the freshly
reset bias is rejected again and never recovers (demo5) */
threshadj = 1.0;
if (P != nullptr && opt->mode > PMODE_DGPS)
{
const int ii = IB_RTK(sat[i], frq, opt);
const int jj = IB_RTK(sat[j], frq, opt);
if (P[ii + rtk->nx * ii] == std::pow(opt->std[0], 2.0) ||
P[jj + rtk->nx * jj] == std::pow(opt->std[0], 2.0))
{
threshadj = INNOVATION_THRESHOLD_INIT_FACTOR;
}
}
/* test innovation, with separate thresholds for phase and code */
if (opt->maxinno[f < nf ? 0 : 1] > 0.0 &&
fabs(v[nv]) > opt->maxinno[f < nf ? 0 : 1] * threshadj)
if (innovation_outlier(i, j, f, v[nv]))
{
/* charge phase outliers only to the non-reference
satellite (demo5): the reference is part of every
tests/unit-tests/signal-processing-blocks/pvt/rtklib_fixed_base_test.cc
+90
View File
@@ -1402,6 +1402,96 @@ TEST_F(RtklibFixedBaseTest, ATransientOutlierDoesNotChargeTheReferenceSatellite)
}
TEST_F(RtklibFixedBaseTest, ACorruptedReferenceDoesNotPoisonHealthySatellites)
{
using namespace rtklib_fixed_base_test_detail;
const double base_position_geodetic[3] = {45.0 * D2R, 8.0 * D2R, 100.0};
double base_position_ecef[3]{};
pos2ecef(base_position_geodetic, base_position_ecef);
const double baseline_enu_m[3] = {8.0, 4.0, 1.0};
double baseline_ecef_m[3]{};
enu2ecef(base_position_geodetic, baseline_enu_m, baseline_ecef_m);
const double rover_position_ecef[3] = {
base_position_ecef[0] + baseline_ecef_m[0],
base_position_ecef[1] + baseline_ecef_m[1],
base_position_ecef[2] + baseline_ecef_m[2]};
prcopt_t options = fixed_base_options();
options.modear = ARMODE_OFF;
options.maxout = 50;
options.err[5] = 52.0;
options.err[6] = 0.005;
auto solver = make_solver_with_options(options);
const std::vector<unsigned int> satellites = select_relative_satellites(
base_position_ecef, rover_position_ecef);
ASSERT_GE(satellites.size(), 6U);
/* Report much stronger signals for the satellite that will jump, forcing
a variance-only selector to use it as the phase reference. Equal jumps
in meters on both bands keep the geometry-free slip detector quiet. */
const unsigned int corrupted_prn = satellites.back();
constexpr int OUTLIER_EPOCH = 6;
constexpr double PHASE_JUMP_M = 300.0;
const double wavelengths_m[2] = {
SPEED_OF_LIGHT_M_S / FREQ1,
SPEED_OF_LIGHT_M_S / FREQ2};
for (int epoch_index = 0; epoch_index <= OUTLIER_EPOCH; ++epoch_index)
{
Synthetic_Relative_Epoch epoch = make_relative_epoch(
*solver, epoch_index, base_position_ecef, rover_position_ecef, satellites);
for (auto& rover_observation : epoch.rover_observations)
{
rover_observation.second.CN0_dB_hz = 20.0;
}
for (unsigned int band = 0; band < 2U; ++band)
{
const auto observation = epoch.rover_observations.find(
static_cast<int>(corrupted_prn * 2U + band));
ASSERT_NE(epoch.rover_observations.end(), observation);
observation->second.CN0_dB_hz = 60.0;
if (epoch_index == OUTLIER_EPOCH)
{
observation->second.Carrier_phase_rads +=
PHASE_JUMP_M / wavelengths_m[band] * 2.0 * GNSS_PI;
}
}
for (obsd_t& base_observation : epoch.base_snapshot.observations)
{
base_observation.SNR[0] = base_observation.SNR[1] = 80U;
if (base_observation.sat == satno(SYS_GPS, static_cast<int>(corrupted_prn)))
{
base_observation.SNR[0] = base_observation.SNR[1] = 240U;
}
}
SCOPED_TRACE(::testing::Message() << "epoch=" << epoch_index);
ASSERT_TRUE(solve(*solver, epoch.rover_observations, &epoch.base_snapshot));
}
EXPECT_EQ(SOLQ_FLOAT, solver->pvt_sol.stat);
EXPECT_EQ(satellites.size() - 1U, static_cast<std::size_t>(solver->pvt_sol.ns));
for (const unsigned int prn : satellites)
{
const auto& state = solver->pvt_ssat[prn - 1U];
for (unsigned int band = 0; band < 2U; ++band)
{
if (prn == corrupted_prn)
{
EXPECT_GT(state.rejc[band], 0U);
EXPECT_EQ(0U, state.vsat[band]);
}
else
{
EXPECT_EQ(0U, state.rejc[band]);
EXPECT_NE(0U, state.vsat[band]);
EXPECT_EQ(0U, state.outc[band]);
}
}
}
}
TEST_F(RtklibFixedBaseTest, MinDropSatsCyclesAPoisonedSatelliteOutOfAmbiguityResolution)
{
using namespace rtklib_fixed_base_test_detail;