feat!: weight the least-squares solve by measurement uncertainty (C/N₀ + elevation) - #69
Open
zsoerenm wants to merge 2 commits into
Open
feat!: weight the least-squares solve by measurement uncertainty (C/N₀ + elevation)#69zsoerenm wants to merge 2 commits into
zsoerenm wants to merge 2 commits into
Conversation
`user_position` let `LsqFit.curve_fit` stop far short of the least-squares
optimum on a warm start. Two defaults conspire:
* `x_tol` is a *relative* step tolerance — the iteration stops once a step
falls below `x_tol·(x_tol + ‖ξ‖)` — and `‖ξ‖` is dominated not by the
position but by the clock bias, which carries the ~2e7 m of common range
the pseudoranges are referred to. The 1e-8 default therefore declares
convergence at a step of ~0.2 m.
* `lambda = 10` damps the first Levenberg-Marquardt steps to a fraction of
the Gauss-Newton step, so the step size crosses that threshold long
before the iterate reaches the optimum.
Seeded a metre from the optimum — the ordinary epoch-to-epoch warm start —
the solve therefore took one ~10 cm step and reported success, returning a
fix 1.6 m away from the one the very same measurements yield from cold, with
post-fit residuals (2.6 m against 1.3 m) belonging to the unconverged
position. The cold path was unaffected: it already ran undamped.
Tighten `x_tol` to 1e-13 (a ~2 µm step, still ~1e3 × the rounding resolution
of `ξ`) and start `lambda` at 1e-8. The tiny non-zero lambda leaves the
iteration effectively Gauss-Newton, which suits a problem this mildly
nonlinear — 2 to 5 iterations to nanometre level from 1 m, 100 km or a cold
start, against 8 to 16 with the default damping or with damping switched off
entirely (`lambda = 0`, where a rejected step can never grow the trust
region) — while keeping LM's machinery available, since a step that fails to
improve still grows lambda from there.
`calc_pvt` cost moves by −35 % to +31 % depending on the epoch (10 to 33 µs
either way): warm starts and 4-satellite fixes get cheaper, the Galileo
5-satellite cold start pays ~6 µs for the iterations it previously skipped.
Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Every satellite admitted to a fix contributed equally, however noisy its pseudorange actually was. Code noise is a strong function of C/N₀ — the coherent-DLL thermal jitter is ~1.2 m at 45 dBHz and ~6.5 m at 30 dBHz on GPS L1 C/A — so one marginal satellite could drag a fix that the strong ones would otherwise have pinned down. Closes #65. `SatelliteState` gains an optional `cn0` (a `dBHz` level, matching `Tracking.estimate_cn0`) and an optional `pseudorange_variance` override for a caller that owns its own error model. As soon as any satellite of an epoch carries either, the position solve minimises `Σ (ρ̂ⱼ − ρⱼ)²/σ²ⱼ` and the velocity solve weights its normal equations by the Doppler's own variance; with neither, the epoch stays the ordinary least squares it was. The variance models (`src/measurement_variance.jl`) follow the standard combined form, `σ² = σ²_UERE + (a/sin El)² + b²·10^(−CN0/10)`: a non-thermal UERE floor, an elevation-mapped term for multipath and the residual of the broadcast atmospheric models, and a thermal term whose coefficient is derived per signal from its chipping rate rather than tuned — so a wider-band signal is credited its sharper correlation peak automatically. σ is clamped to [0.5, 50] m (a supplied variance too), which bounds both a single bad C/N₀ reading's leverage and what weighting can cost the conditioning. The Doppler gets its own FLL-derived variance, since its noise follows the carrier loop. Weighting is applied by pre-whitening residual, Jacobian and directional derivative with √w rather than through `LsqFit`'s `wt` argument, which deprecates a bare inverse-variance vector in 0.16 and would tie the weight convention to the LsqFit version. DOP stays the purely geometric `(HᵀH)⁻¹` — it is what every receiver and textbook means by it, and a weighted DOP would break any downstream comparison silently. The weighted covariance is reported separately as `PVTSolution.accuracy`, a `FormalAccuracy` carrying the 1σ horizontal, vertical, 3D and clock accuracy in metres; with no uncertainty reported it reduces exactly to the familiar DOP × nominal-UERE product. Per satellite, `SatInfo` keeps the raw metre residual (unchanged meaning, weighted or not) and adds the `pseudorange_sigma` it was weighted by, whose ratio is the normalised residual that RAIM (#9) wants. The per-satellite range-rate residuals of v5.0.0 stay raw m/s: the weights scale the normal equations, not the stored measurement, so a satellite that is trusted least still reports its full Doppler disagreement — which is the point of an outlier indicator. The Tracking extension forwards `estimate_cn0` of the ranging signal, so a tracking receiver gets the weighted solve without wiring anything up; the `0.0dBHz` a fresh estimator reports counts as "not reported" rather than as an unusably weak signal. Measured on the GPS L1 fixtures with three of nine satellites at 30 dBHz and the rest at 48 dBHz, each perturbed by the noise its own σ implies: the weighted fix's 3D RMS error over 150 seeded realisations is 6.5 m against 8.2 m unweighted (−21 %), the velocity error −60 %, and the reported `accuracy.position` predicts the weighted spread to within 5 %. BREAKING CHANGE: `PVTSolution` gains an `accuracy` field and `SatInfo` a `pseudorange_sigma` field, so positional construction of either changes arity. Reading them (`pvt.accuracy`, `info.pseudorange_sigma`) is additive. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Codecov Report✅ All modified and coverable lines are covered by tests. Additional details and impacted files@@ Coverage Diff @@
## converge-the-position-solve #69 +/- ##
===============================================================
+ Coverage 98.56% 98.71% +0.14%
===============================================================
Files 7 8 +1
Lines 488 544 +56
===============================================================
+ Hits 481 537 +56
Misses 7 7 ☔ View full report in Codecov by Harness. 🚀 New features to boost your workflow:
|
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Closes #65. Stacked on #68 (
converge-the-position-solve) — that fix is a prerequisite, not a convenience: the weighted refinement solve always starts a little away from the weighted optimum, so without it every weighted epoch stopped ~0.7 m short and the accuracy gain vanished. Review #68 first; this PR's base will retarget tomasteronce it lands.What changes
SatelliteStategains two optional fields, both defaulting tonothing:cn0— adBHzlevel, matchingTracking.estimate_cn0pseudorange_variance— e.g.(3.0m)^2, for a caller that owns its own error modelAs soon as any satellite of an epoch carries either, the position solve minimises
Σ (ρ̂ⱼ − ρⱼ)²/σ²ⱼand the velocity solve weights its normal equations by the Doppler's own variance. With neither, the epoch is the ordinary least squares it was — unweighted behaviour is unchanged, which is what keeps this non-breaking for existing callers.The Tracking extension forwards
estimate_cn0of the ranging signal automatically, so a tracking receiver (GNSSReceiver.jl#118's marginal-C/N₀ backstop, which motivated the issue) gets the weighted solve without wiring anything up.cn0/pseudorange_varianceare overridable there, and the0.0dBHza fresh estimator reports counts as "not reported" rather than as an unusably weak signal.The variance models (
src/measurement_variance.jl)The standard combined form the issue proposes,
σ² = σ²_UERE + (a/sin El)² + b²·10^(−CN0/10):a/sin El(c/f_chip)·√(B_n·d/2)·10^(−CN0/20); the coefficient is derived per signal from its chipping rate, so GPS L5 is credited its 10× sharper correlation peak automatically rather than needing a tuned constantσ is clamped to [0.5, 50] m — a supplied variance too, so a zero/
NaN/absurd value cannot produce an infinite weight. That also bounds the conditioning cost of weighting (cond(√W·H) ≤ (σ_max/σ_min)·cond(H)). The Doppler gets its own FLL-derived variance, since its noise follows the carrier loop, not the DLL; below ~2.87° elevation the σ saturates at the same bound the tropospheric mapping uses.Elevation comes from the reference position the epoch is solved about, exactly as the atmospheric delays do — a metre-accurate position is plenty, so no iterate-to-convergence.
Design decisions from the issue
pvt.dopremains(HᵀH)⁻¹and is unaffected by weighting, so it keeps meaning what every receiver and textbook means by it. The weighted covariance is a separate new field,pvt.accuracy::FormalAccuracy— 1σ horizontal, vertical, 3D and clock accuracy in metres. With no uncertainty reported it reduces exactly to the familiar DOP × nominal-UERE product (tested).SatInfo.residualis still metres, weighted or not, so it is comparable across epochs; the newSatInfo.pseudorange_sigmareports the σ it was weighted by, and their ratio is the normalised residual RAIM (Commit e36b3a6 removed RAIM and previous PVT solution reuse for least squares, is this intentional? #9) wants. v5.0.0'srate_residuallikewise stays raw m/s under weighting.HᵀWHis positive definite exactly whenHᵀHis — the DOP-check-before-velocity-solve invariant survives, and the argument is documented where it is relied on.LsqFit'swt, which deprecates a bare inverse-variance vector in 0.16 and would tie the weight convention to the LsqFit version across the 0.12–0.16 compat range.Does it actually help?
GPS L1 fixtures, three of nine satellites at 30 dBHz and six at 48 dBHz, each perturbed by the noise its own σ implies, 150 seeded realisations, each estimator measured against its own noiseless fix:
and the reported
accuracy.position(6.2 m) predicts the weighted spread to within 5 %. Also tested: a satellite handed a 60 m error and the σ ceiling leaves the fix within 5 % of the four-good-satellite fix; a deterministic ±2σ realisation; the model's monotonicity, clamping and unknown-input handling; the extension's C/N₀ forwarding.test/weighted_least_squares.jlis new (~390 lines, 74 assertions);Randomjoins the test target for the seeded draws. Full suite anddocs/make.jlpass.Deliberately left out
Normalised residuals as a reported field and the RAIM hook (#9) —
pseudorange_sigmais the input they need, so that can land separately. The opt-in real-data integration test (PVT_RUN_INTEGRATION_TEST=true) was not run here; it now exercises weighting on real C/N₀s through the extension and is worth a run before merge.Breaking
PVTSolutiongainsaccuracyandSatInfogainspseudorange_sigma, so positional construction of either changes arity (reading is additive). Same shape of break as v5.0.0'srate_residual.🤖 Generated with Claude Code