From 8ee2ef85c5fbd9bbadbf4f82ead5a29c95191ed4 Mon Sep 17 00:00:00 2001 From: siebc <226531417+siebc@users.noreply.github.com> Date: Mon, 20 Jul 2026 12:40:06 +0000 Subject: [PATCH] feat(track): add VectorPLLAndDLL vector-tracking Doppler estimator A vector-tracking (VDFLL) estimator whose per-satellite loops are closed by an external navigation filter rather than per-satellite loop filters. Configuration-only; per-sat SatVectorPLLAndDLL state lives on TrackedSat and plugs into the shared per-sat update via _process_estimator_driver_signal. With vt_on unset a satellite runs the conventional FLL-assisted scalar PLL/DLL fallback. Once promoted (update_vt_states! / set_vt_on!), the navigation filter's NCO corrections close the loops: code Doppler follows code_freq_update directly, carrier_freq_update drives the carrier filter's FLL branch, and the DLL/FLL discriminators are accumulated for the filter to read (reset_code_discr_acc! / reset_carrier_discr_acc!). The state managers come in all-groups and group-scoped forms so PRNs can be disambiguated across constellations. Availability is the receiver's responsibility, not part of this state. Co-Authored-By: Claude Opus 4.8 (1M context) --- docs/make.jl | 1 + docs/src/vector_tracking.md | 146 +++++++++ src/Tracking.jl | 10 + src/tracking_state.jl | 8 + src/vector_pll_and_dll.jl | 627 ++++++++++++++++++++++++++++++++++++ test/runtests.jl | 1 + test/vector_pll_and_dll.jl | 365 +++++++++++++++++++++ 7 files changed, 1158 insertions(+) create mode 100644 docs/src/vector_tracking.md create mode 100644 src/vector_pll_and_dll.jl create mode 100644 test/vector_pll_and_dll.jl diff --git a/docs/make.jl b/docs/make.jl index acb19b8e..62c9f4f1 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -15,6 +15,7 @@ makedocs( "bit_sync.md", "loop_filter.md", "custom_doppler_estimator.md", + "vector_tracking.md", "correlator.md", "cn0_estimator.md" ] diff --git a/docs/src/vector_tracking.md b/docs/src/vector_tracking.md new file mode 100644 index 00000000..480696b5 --- /dev/null +++ b/docs/src/vector_tracking.md @@ -0,0 +1,146 @@ +# Vector Tracking + +[`VectorPLLAndDLL`](@ref) is a Doppler estimator for **vector tracking** +(a vector delay/frequency-lock loop, VDFLL). Where the +[`ConventionalPLLAndDLL`](@ref) closes each satellite's carrier and code +loops independently with per-satellite loop filters, vector tracking closes +them *centrally*: an external navigation filter — living outside Tracking.jl, +e.g. in GNSSReceiver.jl — combines every satellite's measurements with the +receiver dynamics and feeds back per-satellite NCO corrections. A satellite +in a deep fade keeps being steered by the solution the healthy satellites +constrain, which is what gives vector tracking its robustness under weak +signal and high dynamics. + +Tracking.jl owns only the signal-path half of that loop: it correlates, +forms discriminators, and applies the corrections the navigation filter +hands back. `VectorPLLAndDLL` is the interface between the two. + +## The per-integration contract + +`VectorPLLAndDLL` is configuration-only; the per-satellite state lives on +each [`TrackedSat`](@ref) as a `SatVectorPLLAndDLL` (seeded through +[`init_estimator_state`](@ref), like every estimator — see +[Custom Doppler Estimator](custom_doppler_estimator.md)). Each time a +satellite's estimator-driver signal (`signals[1]`) completes an integration, +`VectorPLLAndDLL` does one of two things depending on that satellite's +`vt_on` flag: + + - **`vt_on = false` — scalar fallback.** The satellite runs an ordinary + FLL-assisted PLL and DLL, identical to + [`ConventionalAssistedPLLAndDLL`](@ref) (same default loop-filter types + and the same auto-sized loop bandwidths). This is the pull-in mode a + freshly acquired satellite tracks in until it is promoted into the + vector loop. + + - **`vt_on = true` — vector closure.** The navigation filter drives the + NCOs instead of the local loop filters: + + * the **code Doppler follows `code_freq_update` directly** (the DLL + loop filter is bypassed and holds its state); + * the **carrier's FLL branch is driven by `carrier_freq_update`** + while the PLL branch still runs on the satellite's own phase + discriminator — so the carrier is a vector *frequency* lock loop + with a retained local *phase* lock. (This needs the FLL-assisted + `ThirdOrderAssistedBilinearLF`, the default carrier filter; with a + plain PLL filter the `carrier_freq_update` has no input path and the + vector carrier closure degrades to PLL-only.) + + In this mode the DLL and FLL discriminator outputs and the prompt + magnitude are **accumulated** on the per-sat state for the navigation + filter to read and reset. + +The satellite-shared carrier/code Doppler is always updated through the same +carrier-aiding (`aid_dopplers`) used by the conventional estimator, and +the same `1/N` loop-bandwidth scaling applies when a signal integrates `N` +primary code blocks coherently. + +## The receiver-side loop + +A navigation filter drives `VectorPLLAndDLL` through the exported state +managers. A typical iteration, after [`track!`](@ref) has produced new +correlations: + +```julia +# 1. Promote satellites that have pulled in, and flag the ones currently +# in an outage. `prns_in_lock` is whatever the receiver decides is +# usable (e.g. a C/N0 threshold). +update_vt_states!(track_state, prns_in_lock) + +# 2. Read the accumulated discriminator outputs the estimator collected +# for each vector-loop satellite, then reset the accumulators so the +# next block accumulates afresh. +for (prn, sat) in pairs(get_sat_states(track_state)) + state = get_doppler_estimator_state(sat) + state.vt_on || continue + code_err = mean_code_discriminator(state) # chips, or `nothing` if no data + carrier_err = mean_carrier_discriminator(state) # Hz, or `nothing` if no data + # … feed the mean measurements into the navigation filter … +end +reset_code_discr_acc!(track_state) +reset_carrier_discr_acc!(track_state) + +# 3. Run the navigation filter, then feed its per-satellite NCO +# corrections back. Both setters take anything indexable by PRN +# (e.g. a Dictionaries.Dictionary) with an entry for every vector-loop +# satellite; satellites still in the scalar fallback are skipped. +set_code_freq_updates!(track_state, code_freq_updates) +set_carrier_freq_updates!(track_state, carrier_freq_updates) +``` + +The accumulators are stored as `(count, sum)` tuples; +[`mean_code_discriminator`](@ref) / [`mean_carrier_discriminator`](@ref) apply +the averaging convention (`sum / count`, returning `nothing` when nothing has +accumulated) in one place, so consumers don't each re-implement the divide and +the `count == 0` guard. Reading and resetting are deliberately separate calls +so the filter can read at its own (typically slower) rate than `track!`. + +### Multi-constellation addressing + +Each state manager has an all-groups form and a **group-scoped** form that +takes a group selector (`Symbol`, `Integer`, or `Val`) as its first argument +after `track_state`: + +```julia +update_vt_states!(track_state, :gps, gps_prns_in_lock) +set_code_freq_updates!(track_state, :galileo, galileo_code_updates) +``` + +In a multi-constellation receiver a PRN alone is ambiguous — GPS PRN 5 and +Galileo PRN 5 are different satellites in different groups — so address each +constellation's group explicitly. The all-groups form matches a PRN in every +group and is only unambiguous for a single-group `TrackState`. + +Promotion is one-directional: [`update_vt_states!`](@ref) only ever *joins* +satellites to the vector loop. A satellite in an outage keeps being steered by +the navigation filter from the shared solution; deciding whether its (now +uninformative) discriminator outputs should feed the measurement update is the +navigation filter's responsibility, tracked on the receiver side rather than +on the estimator state. Use [`set_vt_on!`](@ref) to force loop membership on +or off manually. + +## Resetting + +[`reset_loop_filters!`](@ref) zeroes the loop-filter integrators, the +discriminator accumulators, **and** the NCO corrections, re-seeding the loop +from the satellite's current (converged) Doppler while preserving the +`vt_on` flag and any per-satellite bandwidth override. The +NCO corrections are zeroed because the re-seeded Doppler already contains the +last correction — keeping it would apply it twice. After a reset the +navigation filter must re-issue its corrections via +[`set_code_freq_updates!`](@ref) / [`set_carrier_freq_updates!`](@ref) before +the next vector-closed integration. + +## API reference + +```@docs +VectorPLLAndDLL +Tracking.SatVectorPLLAndDLL +update_vt_states! +set_vt_on! +set_code_freq_updates! +set_carrier_freq_updates! +reset_code_discr_acc! +reset_carrier_discr_acc! +mean_code_discriminator +mean_carrier_discriminator +``` diff --git a/src/Tracking.jl b/src/Tracking.jl index b861cc45..06266066 100644 --- a/src/Tracking.jl +++ b/src/Tracking.jl @@ -77,6 +77,15 @@ export get_early, TwoBitThreadedDownconvertAndCorrelator, ConventionalPLLAndDLL, ConventionalAssistedPLLAndDLL, + VectorPLLAndDLL, + update_vt_states!, + set_vt_on!, + reset_code_discr_acc!, + reset_carrier_discr_acc!, + mean_code_discriminator, + mean_carrier_discriminator, + set_code_freq_updates!, + set_carrier_freq_updates!, DefaultPostCorrFilter, TrackState, add_satellite!, @@ -222,6 +231,7 @@ include("downconvert_and_correlate_int16.jl") include("downconvert_and_correlate_onebit.jl") include("downconvert_and_correlate_twobit.jl") include("conventional_pll_and_dll.jl") +include("vector_pll_and_dll.jl") include("tracking_state.jl") include("track.jl") diff --git a/src/tracking_state.jl b/src/tracking_state.jl index 4934c851..a3e31330 100644 --- a/src/tracking_state.jl +++ b/src/tracking_state.jl @@ -987,6 +987,14 @@ filter's integrator state is not portable across the change in update interval can drag the loop out of lock; the converged Doppler is the right seed for the new, longer integration. +For [`VectorPLLAndDLL`](@ref) the re-seed additionally zeroes the +externally-supplied NCO corrections (`code_freq_update` / `carrier_freq_update`) +and the discriminator accumulators — the converged Doppler already folds in the +last correction, so keeping it would apply it twice. A navigation filter must +therefore **re-issue** its corrections via [`set_code_freq_updates!`](@ref) / +[`set_carrier_freq_updates!`](@ref) after resetting a vector-loop satellite. +The `vt_on` flag and any per-satellite bandwidth override are preserved. + Addressed like the per-signal accessors — no satellite id resets every satellite in `track_state`; `(group, prn)` or (single-group) `prn` resets one. Mutates `track_state` in place and returns it. Works for any diff --git a/src/vector_pll_and_dll.jl b/src/vector_pll_and_dll.jl new file mode 100644 index 00000000..debe46e3 --- /dev/null +++ b/src/vector_pll_and_dll.jl @@ -0,0 +1,627 @@ +""" +Per-satellite state for the vector PLL and DLL Doppler estimator +([`VectorPLLAndDLL`](@ref)). + +On top of the conventional loop-filter state it carries the +vector-tracking (VT) interface to an external navigation filter +(e.g. GNSSReceiver.jl's VDFLL): + + - `code_discr_acc` / `carrier_discr_acc`: `(count, sum)` accumulators of + the DLL discriminator (chips) and the FLL discriminator (Hz) since the + navigation filter last read and reset them + ([`reset_code_discr_acc!`](@ref) / [`reset_carrier_discr_acc!`](@ref)). + Only accumulated while `vt_on`. + - `code_freq_update` / `carrier_freq_update`: the NCO corrections the + navigation filter feeds back ([`set_code_freq_updates!`](@ref), + [`set_carrier_freq_updates!`](@ref)). While `vt_on`, they replace the + scalar DLL loop-filter output and the FLL branch of the carrier loop + filter respectively. + - `vt_on`: whether the navigation filter controls this satellite's NCOs. + While `false` the satellite runs a conventional (scalar) PLL/DLL as a + fallback and nothing is accumulated. Promoted by [`update_vt_states!`](@ref) + / [`set_vt_on!`](@ref). +""" +@kwdef struct SatVectorPLLAndDLL{CA<:AbstractLoopFilter,CO<:AbstractLoopFilter} + init_carrier_doppler::typeof(1.0Hz) + init_code_doppler::typeof(1.0Hz) + carrier_loop_filter::CA = ThirdOrderAssistedBilinearLF() + code_loop_filter::CO = SecondOrderBilinearLF() + carrier_loop_filter_bandwidth::typeof(1.0Hz) = 18.0Hz + code_loop_filter_bandwidth::typeof(1.0Hz) = 1.0Hz + code_discr_acc::Tuple{Int,Float64} = (0, 0.0) + code_freq_update::typeof(0.0Hz) = 0.0Hz + carrier_discr_acc::Tuple{Int,typeof(0.0Hz)} = (0, 0.0Hz) + carrier_freq_update::typeof(0.0Hz) = 0.0Hz + vt_on::Bool = false +end + +function SatVectorPLLAndDLL( + sat::TrackedSat, + carrier_loop_filter::CA, + code_loop_filter::CO; + carrier_loop_filter_bandwidth::typeof(1.0Hz) = 18.0Hz, + code_loop_filter_bandwidth::typeof(1.0Hz) = 1.0Hz, +) where {CA<:AbstractLoopFilter,CO<:AbstractLoopFilter} + SatVectorPLLAndDLL(; + init_carrier_doppler = sat.carrier_doppler, + init_code_doppler = sat.code_doppler, + carrier_loop_filter, + code_loop_filter, + carrier_loop_filter_bandwidth, + code_loop_filter_bandwidth, + ) +end + +function SatVectorPLLAndDLL( + sat_vector_pll_and_dll::SatVectorPLLAndDLL{CA,CO}; + carrier_loop_filter::Maybe{CA} = nothing, + code_loop_filter::Maybe{CO} = nothing, + carrier_loop_filter_bandwidth::Maybe{typeof(1.0Hz)} = nothing, + code_loop_filter_bandwidth::Maybe{typeof(1.0Hz)} = nothing, + code_discr_acc::Maybe{Tuple{Int,Float64}} = nothing, + code_freq_update::Maybe{typeof(0.0Hz)} = nothing, + carrier_discr_acc::Maybe{Tuple{Int,typeof(0.0Hz)}} = nothing, + carrier_freq_update::Maybe{typeof(0.0Hz)} = nothing, + vt_on::Maybe{Bool} = nothing, +) where {CA<:AbstractLoopFilter,CO<:AbstractLoopFilter} + SatVectorPLLAndDLL{CA,CO}( + sat_vector_pll_and_dll.init_carrier_doppler, + sat_vector_pll_and_dll.init_code_doppler, + isnothing(carrier_loop_filter) ? sat_vector_pll_and_dll.carrier_loop_filter : + carrier_loop_filter, + isnothing(code_loop_filter) ? sat_vector_pll_and_dll.code_loop_filter : + code_loop_filter, + isnothing(carrier_loop_filter_bandwidth) ? + sat_vector_pll_and_dll.carrier_loop_filter_bandwidth : + carrier_loop_filter_bandwidth, + isnothing(code_loop_filter_bandwidth) ? + sat_vector_pll_and_dll.code_loop_filter_bandwidth : code_loop_filter_bandwidth, + isnothing(code_discr_acc) ? sat_vector_pll_and_dll.code_discr_acc : code_discr_acc, + isnothing(code_freq_update) ? sat_vector_pll_and_dll.code_freq_update : + code_freq_update, + isnothing(carrier_discr_acc) ? sat_vector_pll_and_dll.carrier_discr_acc : + carrier_discr_acc, + isnothing(carrier_freq_update) ? sat_vector_pll_and_dll.carrier_freq_update : + carrier_freq_update, + isnothing(vt_on) ? sat_vector_pll_and_dll.vt_on : vt_on, + ) +end + +""" +$(SIGNATURES) + +Vector-tracking Phase-Locked Loop (PLL) and Delay-Locked Loop (DLL) Doppler +estimator. Configuration-only — per-satellite state lives in each +[`TrackedSat`](@ref) wrapper as a [`SatVectorPLLAndDLL`](@ref), produced via +[`init_estimator_state`](@ref). + +In vector tracking, the per-satellite tracking loops are closed centrally by +a navigation filter (living outside this package, e.g. in GNSSReceiver.jl) +instead of by per-satellite loop filters. The division of labor per +integration: + + - This estimator accumulates each satellite's DLL / FLL discriminator + outputs for the navigation filter to consume (and reset via + [`reset_code_discr_acc!`](@ref) / [`reset_carrier_discr_acc!`](@ref)). + - The navigation filter feeds NCO corrections back via + [`set_code_freq_updates!`](@ref) / [`set_carrier_freq_updates!`](@ref). + While a satellite's `vt_on` flag is set, its code Doppler follows the + navigation filter's `code_freq_update` directly and the FLL branch of + the FLL-assisted carrier loop filter is driven by the navigation + filter's `carrier_freq_update` (a vector delay / frequency lock loop) + while the PLL branch still runs on the satellite's own discriminator. + - Satellites with `vt_on` unset (fresh from acquisition, before + [`update_vt_states!`](@ref) promotes them) run a conventional scalar + PLL/DLL as a fallback. + +Type parameters `CA` and `CO` select the carrier and code loop filter types. +The FLL-assisted `ThirdOrderAssistedBilinearLF` carrier filter is the +default — with any non-assisted filter the navigation filter's +`carrier_freq_update` has no input path into the carrier loop. + +Each bandwidth field is `Maybe{typeof(1.0Hz)}`: a `nothing` field (the +default) means **auto** — [`init_estimator_state`](@ref) sizes the bandwidth +per satellite from that sat's estimator-driver signal (`signals[1]`) via +[`default_carrier_loop_filter_bandwidth`](@ref) / +[`default_code_loop_filter_bandwidth`](@ref) — the same sizing as the +conventional estimator, which the scalar fallback loop is. Like the +conventional estimator, the effective bandwidth is scaled by `1/N` at filter +time when a signal coherently integrates `N` primary code blocks. +""" +struct VectorPLLAndDLL{CA<:AbstractLoopFilter,CO<:AbstractLoopFilter} <: + AbstractDopplerEstimator + carrier_loop_filter_bandwidth::Maybe{typeof(1.0Hz)} + code_loop_filter_bandwidth::Maybe{typeof(1.0Hz)} +end + +function VectorPLLAndDLL( + ::Type{CA} = ThirdOrderAssistedBilinearLF, + ::Type{CO} = SecondOrderBilinearLF; + carrier_loop_filter_bandwidth::Maybe{typeof(1.0Hz)} = nothing, + code_loop_filter_bandwidth::Maybe{typeof(1.0Hz)} = nothing, +) where {CA<:AbstractLoopFilter,CO<:AbstractLoopFilter} + VectorPLLAndDLL{CA,CO}(carrier_loop_filter_bandwidth, code_loop_filter_bandwidth) +end + +# Kwarg-update constructor for tweaking bandwidths in place. +function VectorPLLAndDLL( + pll_and_dll::VectorPLLAndDLL{CA,CO}; + carrier_loop_filter_bandwidth::Maybe{typeof(1.0Hz)} = nothing, + code_loop_filter_bandwidth::Maybe{typeof(1.0Hz)} = nothing, +) where {CA<:AbstractLoopFilter,CO<:AbstractLoopFilter} + VectorPLLAndDLL{CA,CO}( + isnothing(carrier_loop_filter_bandwidth) ? + pll_and_dll.carrier_loop_filter_bandwidth : carrier_loop_filter_bandwidth, + isnothing(code_loop_filter_bandwidth) ? pll_and_dll.code_loop_filter_bandwidth : + code_loop_filter_bandwidth, + ) +end + +""" +$(SIGNATURES) + +Build the per-satellite estimator state stored in a [`TrackedSat`](@ref) for a +satellite tracked under [`VectorPLLAndDLL`](@ref). Auto bandwidths (`nothing` +on the estimator) are resolved here, per satellite, from the sat's +estimator-driver signal (`signals[1]`). + +New satellites start with `vt_on = false` — the scalar fallback loop — +until [`update_vt_states!`](@ref) (or [`set_vt_on!`](@ref)) promotes them +into the vector loop. +""" +function init_estimator_state( + estimator::VectorPLLAndDLL{CA,CO}, + sat::TrackedSat, +) where {CA<:AbstractLoopFilter,CO<:AbstractLoopFilter} + carrier_loop_filter = constructorof(CA)() + code_loop_filter = constructorof(CO)() + driver_signal = first(sat.signals).signal + carrier_loop_filter_bandwidth = + isnothing(estimator.carrier_loop_filter_bandwidth) ? + default_carrier_loop_filter_bandwidth(driver_signal) : + estimator.carrier_loop_filter_bandwidth + code_loop_filter_bandwidth = + isnothing(estimator.code_loop_filter_bandwidth) ? + default_code_loop_filter_bandwidth(driver_signal) : + estimator.code_loop_filter_bandwidth + SatVectorPLLAndDLL(; + init_carrier_doppler = sat.carrier_doppler, + init_code_doppler = sat.code_doppler, + carrier_loop_filter, + code_loop_filter, + carrier_loop_filter_bandwidth, + code_loop_filter_bandwidth, + ) +end + +# Re-seed hook used by `reset_loop_filters!`: zero the loop-filter +# integrators, the discriminator accumulators, and the NCO corrections, and +# re-seed the init Dopplers from the sat's current (converged) Dopplers. +# The NCO corrections must be zeroed together with the init Dopplers: the +# current Dopplers already contain the last correction, so keeping it would +# apply it twice after the re-seed. Per-sat bandwidth overrides and the +# `vt_on` flag survive the reset. +function _reset_estimator_state( + ::VectorPLLAndDLL, + sat::TrackedSat{<:Tuple{Vararg{TrackedSignal}},<:SatVectorPLLAndDLL}, +) + state = sat.doppler_estimator_state + SatVectorPLLAndDLL(; + init_carrier_doppler = sat.carrier_doppler, + init_code_doppler = sat.code_doppler, + carrier_loop_filter = constructorof(typeof(state.carrier_loop_filter))(), + code_loop_filter = constructorof(typeof(state.code_loop_filter))(), + carrier_loop_filter_bandwidth = state.carrier_loop_filter_bandwidth, + code_loop_filter_bandwidth = state.code_loop_filter_bandwidth, + vt_on = state.vt_on, + ) +end + +# Carrier loop filtering with an explicit FLL-branch input: the raw FLL +# discriminator in the scalar fallback, or the navigation filter's +# `carrier_freq_update` under vector closure. Only the FLL-assisted filter +# has an input path for it; any other filter runs on the PLL discriminator +# alone (and the vector carrier closure degrades to PLL-only). +function _filter_vector_carrier_loop( + carrier_loop_filter::ThirdOrderAssistedBilinearLF, + pll_discriminator, + fll_input, + integration_time, + loop_bandwidth, +) + filter_loop( + carrier_loop_filter, + (pll_discriminator, fll_input), + integration_time, + loop_bandwidth, + ) +end + +function _filter_vector_carrier_loop( + carrier_loop_filter::AbstractLoopFilter, + pll_discriminator, + fll_input, + integration_time, + loop_bandwidth, +) + filter_loop(carrier_loop_filter, pll_discriminator, integration_time, loop_bandwidth) +end + +# Process the estimator-driver signal (signals[1]) under vector tracking. +# Same structure as the conventional method (dispatching on the per-sat +# state type plugs this into the shared `_update_tracked_sat_doppler`), but +# with the vector closure: while `vt_on`, the navigation filter's NCO +# corrections replace the scalar DLL output and the FLL branch input, and +# the discriminator outputs are accumulated for the navigation filter. +@inline function _process_estimator_driver_signal( + tracked_signal::TrackedSignal, + sat::TrackedSat, + pll_and_dll_state::SatVectorPLLAndDLL, + sampling_frequency, + driver_carrier_phase::Real = 0.0, +) + if !tracked_signal.is_integration_completed || tracked_signal.integrated_samples == 0 + return tracked_signal, pll_and_dll_state, sat.carrier_doppler, sat.code_doppler + end + signal = tracked_signal.signal + # The previous fully-integrated prompt must be read off the *old* + # signal — the shared advance overwrites it with this integration's. + previous_prompt = get_last_fully_integrated_filtered_prompt(tracked_signal) + integration_time = tracked_signal.integrated_samples / sampling_frequency + new_signal, filtered_correlator, integrated_code_blocks = + _advance_signal_after_integration( + tracked_signal, + sat.prn, + sampling_frequency, + driver_carrier_phase, + ) + + # Same 1/N effective-bandwidth scaling as the conventional estimator — + # holds the loop's BL·Δt stability product at its single-period value + # when a signal coherently integrates N primary code blocks. + carrier_bandwidth = + pll_and_dll_state.carrier_loop_filter_bandwidth / integrated_code_blocks + code_bandwidth = pll_and_dll_state.code_loop_filter_bandwidth / integrated_code_blocks + + pll_discriminator = pll_disc(signal, filtered_correlator) + fll_discriminator = + fll_disc(signal, filtered_correlator, previous_prompt, integration_time) + dll_discriminator = + dll_disc(signal, filtered_correlator, sat.code_doppler, sampling_frequency) + + if pll_and_dll_state.vt_on + # Vector loop closure: accumulate the discriminator outputs for the + # navigation filter and apply its NCO corrections — `code_freq_update` + # directly, `carrier_freq_update` through the FLL branch of the + # carrier loop filter (the PLL branch still runs on this satellite's + # own discriminator). The code loop filter is bypassed and keeps its + # state. + code_discr_acc = pll_and_dll_state.code_discr_acc .+ (1, dll_discriminator) + carrier_discr_acc = pll_and_dll_state.carrier_discr_acc .+ (1, fll_discriminator) + code_freq_update = pll_and_dll_state.code_freq_update + code_loop_filter = pll_and_dll_state.code_loop_filter + fll_input = pll_and_dll_state.carrier_freq_update + else + # Scalar tracking fallback — a conventional FLL-assisted PLL and DLL. + code_discr_acc = pll_and_dll_state.code_discr_acc + carrier_discr_acc = pll_and_dll_state.carrier_discr_acc + code_freq_update, code_loop_filter = filter_loop( + pll_and_dll_state.code_loop_filter, + dll_discriminator, + integration_time, + code_bandwidth, + ) + fll_input = fll_discriminator + end + carrier_freq_update, carrier_loop_filter = _filter_vector_carrier_loop( + pll_and_dll_state.carrier_loop_filter, + pll_discriminator, + fll_input, + integration_time, + carrier_bandwidth, + ) + carrier_doppler, code_doppler = aid_dopplers( + signal, + pll_and_dll_state.init_carrier_doppler, + pll_and_dll_state.init_code_doppler, + carrier_freq_update, + code_freq_update, + ) + # `carrier_freq_update` is deliberately NOT stored back into the state: + # the stored field carries the navigation filter's correction, which the + # carrier-loop-filter output would otherwise overwrite between two + # `set_carrier_freq_updates!` calls. + new_doppler_estimator_state = SatVectorPLLAndDLL( + pll_and_dll_state; + carrier_loop_filter, + code_loop_filter, + code_discr_acc, + code_freq_update, + carrier_discr_acc, + ) + return new_signal, new_doppler_estimator_state, carrier_doppler, code_doppler +end + +""" +$(SIGNATURES) + +Estimate Dopplers and filter prompts for all satellites where the correlation +has reached the end of the code or multiples of that, using the vector +PLL and DLL implementation — see [`VectorPLLAndDLL`](@ref) for how the +per-satellite loops are closed. In the case that the correlation hasn't +reached the end, e.g. in the case the incoming signal did not provide enough +samples, the state is passed through unchanged. +""" +function estimate_dopplers_and_filter_prompt( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + measurements::BandMeasurements, +) + # Detach the slot *values* (sharing the key set) then delegate to the + # in-place form — same key-sharing copy the conventional estimator uses; + # see its `estimate_dopplers_and_filter_prompt` for the rationale. + new_track_state = + TrackState(track_state; groups = _copy_groups_slot_vectors(track_state.groups)) + estimate_dopplers_and_filter_prompt!(new_track_state, measurements) +end + +""" +$(SIGNATURES) + +In-place version of [`estimate_dopplers_and_filter_prompt`](@ref) for the +vector PLL and DLL estimator. +""" +function estimate_dopplers_and_filter_prompt!( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + measurements::BandMeasurements, +) + _foreach_group!(_est_one_group!, track_state.groups, measurements) + return track_state +end + +# Shared in-place walker for the vector-tracking state managers below: +# overwrite each satellite's `doppler_estimator_state` with the per-sat rule +# `f(sat, sat.doppler_estimator_state, args...)`, threading each manager's own +# arguments (`prns`, a freq-update table, …) through unchanged — the same +# named-worker-plus-trailing-args pattern `_foreach_group!` uses. `f` must +# return the same concrete `SatVectorPLLAndDLL` type (guaranteed by the +# kwarg-update constructor). +# +# Two entry points give the managers their two addressing forms: +# `_map_vt_states!` walks every group; `_map_vt_states_in_group!` walks only the +# addressed group (Symbol / Integer / Val). Keeping them as distinct names — +# rather than one function overloaded on the group position — is what lets a +# threaded `Symbol`/`Integer` argument not be mistaken for a group selector; +# `Bool <: Integer` makes that a real hazard for `set_vt_on!`. The group-scoped +# form is what makes the managers usable in a multi-constellation receiver, +# where PRNs alone are ambiguous (GPS PRN 5 and Galileo PRN 5 are different +# satellites in different groups). +@inline function _map_vt_states!( + f::F, + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + args::Vararg{Any,N}, +) where {F,N} + _foreach_group!(_map_vt_states_one_group!, track_state.groups, f, args...) + return track_state +end + +@inline function _map_vt_states_in_group!( + f::F, + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + group::Union{Symbol,Integer,Val}, + args::Vararg{Any,N}, +) where {F,N} + _map_vt_states_one_group!(_index_group(track_state.groups, group), f, args...) + return track_state +end + +@inline function _map_vt_states_one_group!( + g::SignalGroup, + f::F, + args::Vararg{Any,N}, +) where {F,N} + vals = g.satellites.values + @inbounds for i in eachindex(vals) + sat = vals[i] + vals[i] = TrackedSat( + sat; + doppler_estimator_state = f(sat, sat.doppler_estimator_state, args...), + ) + end + return nothing +end + +# The per-satellite promotion rule of `update_vt_states!`. Promotion is +# one-directional: a satellite whose PRN is in lock and is not yet in the +# vector loop joins it; nothing is ever demoted here (use `set_vt_on!` to drop +# a satellite). +_update_vt_membership(sat, state, prns_in_lock) = + !state.vt_on && sat.prn in prns_in_lock ? SatVectorPLLAndDLL(state; vt_on = true) : + state + +""" +$(SIGNATURES) + +Promote every satellite whose PRN is in `prns_in_lock` (the set the receiver +currently considers usable, e.g. from a carrier-to-noise-density threshold) +into the vector loop by setting its `vt_on` flag; a satellite keeps running +the scalar fallback loop until it appears here. Promotion is one-directional — +a satellite already in the vector loop is left untouched, and none is ever +demoted (use [`set_vt_on!`](@ref) to drop one). + +The two-argument form walks every group; pass a `group` (Symbol or index) to +address a single group — required in a multi-constellation receiver, where a +PRN alone is ambiguous across groups. + +Mutates `track_state` in place and returns it. +""" +function update_vt_states!( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + prns_in_lock, +) + _map_vt_states!(_update_vt_membership, track_state, prns_in_lock) +end + +function update_vt_states!( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + group::Union{Symbol,Integer,Val}, + prns_in_lock, +) + _map_vt_states_in_group!(_update_vt_membership, track_state, group, prns_in_lock) +end + +""" +$(SIGNATURES) + +Set the `vt_on` flag to `vt_on` for every satellite whose PRN is in `prns` — +the manual override for [`update_vt_states!`](@ref)'s automatic promotion. +Walks every group, or only `group` when one is given (the unambiguous form +for multi-constellation receivers). Mutates `track_state` in place and +returns it. +""" +function set_vt_on!( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + vt_on::Bool, + prns, +) + _map_vt_states!(_set_sat_vt_on, track_state, vt_on, prns) +end + +function set_vt_on!( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + group::Union{Symbol,Integer,Val}, + vt_on::Bool, + prns, +) + _map_vt_states_in_group!(_set_sat_vt_on, track_state, group, vt_on, prns) +end + +_set_sat_vt_on(sat, state, vt_on, prns) = + sat.prn in prns ? SatVectorPLLAndDLL(state; vt_on) : state + +""" +$(SIGNATURES) + +Reset the code (DLL) discriminator accumulator of every satellite in the +vector loop — called by the navigation filter after it has consumed the +accumulated values. Mutates `track_state` in place and returns it. +""" +function reset_code_discr_acc!(track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}) + _map_vt_states!(_reset_sat_code_discr_acc, track_state) +end + +_reset_sat_code_discr_acc(_sat, state) = + state.vt_on ? SatVectorPLLAndDLL(state; code_discr_acc = (0, 0.0)) : state + +""" +$(SIGNATURES) + +Reset the carrier (FLL) discriminator accumulator of every satellite in the +vector loop — called by the navigation filter after it has consumed the +accumulated values. Mutates `track_state` in place and returns it. +""" +function reset_carrier_discr_acc!(track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}) + _map_vt_states!(_reset_sat_carrier_discr_acc, track_state) +end + +_reset_sat_carrier_discr_acc(_sat, state) = + state.vt_on ? SatVectorPLLAndDLL(state; carrier_discr_acc = (0, 0.0Hz)) : state + +""" +$(SIGNATURES) + +Mean DLL (code) discriminator accumulated on `state` since the last +[`reset_code_discr_acc!`](@ref), in chips, or `nothing` if nothing has been +accumulated yet (the `(count, sum)` accumulator's `count` is 0). This is the +single place the accumulator's averaging convention lives — read it here +rather than dividing `code_discr_acc` by hand, so a change to how the +accumulator is stored can't silently diverge between consumers. +""" +function mean_code_discriminator(state::SatVectorPLLAndDLL) + count, discr_sum = state.code_discr_acc + count == 0 ? nothing : discr_sum / count +end + +""" +$(SIGNATURES) + +Mean FLL (carrier) discriminator accumulated on `state` since the last +[`reset_carrier_discr_acc!`](@ref), in Hz, or `nothing` if nothing has been +accumulated yet (`count == 0`). The carrier counterpart to +[`mean_code_discriminator`](@ref). +""" +function mean_carrier_discriminator(state::SatVectorPLLAndDLL) + count, discr_sum = state.carrier_discr_acc + count == 0 ? nothing : discr_sum / count +end + +""" +$(SIGNATURES) + +Feed the navigation filter's code-frequency NCO corrections back into the +tracking loops. `code_freq_updates` maps PRN to the correction (anything +indexable by PRN, e.g. a `Dictionary`) and must have an entry for every +satellite in the vector loop; satellites with `vt_on` unset are skipped. +Walks every group, or only `group` when one is given — the group-scoped form +is required in a multi-constellation receiver, where each group carries its +own corrections and PRNs collide across groups. Mutates `track_state` in +place and returns it. +""" +function set_code_freq_updates!( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + code_freq_updates, +) + _map_vt_states!(_set_sat_code_freq_update, track_state, code_freq_updates) +end + +function set_code_freq_updates!( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + group::Union{Symbol,Integer,Val}, + code_freq_updates, +) + _map_vt_states_in_group!( + _set_sat_code_freq_update, + track_state, + group, + code_freq_updates, + ) +end + +_set_sat_code_freq_update(sat, state, code_freq_updates) = + state.vt_on ? SatVectorPLLAndDLL(state; code_freq_update = code_freq_updates[sat.prn]) : + state + +""" +$(SIGNATURES) + +Feed the navigation filter's carrier-frequency NCO corrections back into the +tracking loops. `carrier_freq_updates` maps PRN to the correction (anything +indexable by PRN, e.g. a `Dictionary`) and must have an entry for every +satellite in the vector loop; satellites with `vt_on` unset are skipped. +Walks every group, or only `group` when one is given — the group-scoped form +is required in a multi-constellation receiver, where each group carries its +own corrections and PRNs collide across groups. Mutates `track_state` in +place and returns it. +""" +function set_carrier_freq_updates!( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + carrier_freq_updates, +) + _map_vt_states!(_set_sat_carrier_freq_update, track_state, carrier_freq_updates) +end + +function set_carrier_freq_updates!( + track_state::TrackState{<:SignalGroups,<:VectorPLLAndDLL}, + group::Union{Symbol,Integer,Val}, + carrier_freq_updates, +) + _map_vt_states_in_group!( + _set_sat_carrier_freq_update, + track_state, + group, + carrier_freq_updates, + ) +end + +_set_sat_carrier_freq_update(sat, state, carrier_freq_updates) = + state.vt_on ? + SatVectorPLLAndDLL(state; carrier_freq_update = carrier_freq_updates[sat.prn]) : state diff --git a/test/runtests.jl b/test/runtests.jl index dce265ad..e6b62f75 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,5 +1,6 @@ include("aqua.jl") include("conventional_pll_and_dll.jl") +include("vector_pll_and_dll.jl") include("sat_state.jl") include("sample_parameters.jl") include("downconvert_and_correlate.jl") diff --git a/test/vector_pll_and_dll.jl b/test/vector_pll_and_dll.jl new file mode 100644 index 00000000..a582d97a --- /dev/null +++ b/test/vector_pll_and_dll.jl @@ -0,0 +1,365 @@ +module VectorPLLAndDLLTest + +using Test: @test, @testset, @inferred +using Unitful: Hz +using GNSSSignals: GPSL1CA, GalileoE1B, get_code_center_frequency_ratio +using TrackingLoopFilters: ThirdOrderAssistedBilinearLF, SecondOrderBilinearLF, filter_loop +using StaticArrays: SVector +using Dictionaries: dictionary +using Tracking: + SatVectorPLLAndDLL, + VectorPLLAndDLL, + ConventionalAssistedPLLAndDLL, + TrackedSignal, + TrackedSat, + TrackState, + BandMeasurement, + init_estimator_state, + estimate_dopplers_and_filter_prompt, + get_carrier_doppler, + get_code_doppler, + get_sat_state, + add_satellite!, + get_doppler_estimator_state, + get_default_correlator, + update_accumulator, + dll_disc, + pll_disc, + update_vt_states!, + set_vt_on!, + reset_code_discr_acc!, + reset_carrier_discr_acc!, + mean_code_discriminator, + mean_carrier_discriminator, + set_code_freq_updates!, + set_carrier_freq_updates!, + reset_loop_filters! + +_meas_l1(fs) = (L1 = BandMeasurement(ComplexF64[], fs),) + +# Rebuild `sat` with its driver signal marked as fully integrated with the +# given correlator accumulators, so `estimate_dopplers_and_filter_prompt` +# runs the loop-filter update. +function _with_full_integration(sat, accumulators, num_samples) + correlator = + update_accumulator(get_default_correlator(GPSL1CA()), SVector(accumulators...)) + TrackedSat( + sat; + signals = ( + TrackedSignal( + only(sat.signals); + is_integration_completed = true, + integrated_samples = num_samples, + correlator, + ), + ), + ) +end + +# Swap in a modified per-sat estimator state (same concrete type). +_with_state(sat, state) = TrackedSat(sat; doppler_estimator_state = state) + +@testset "Satellite Vector PLL and DLL" begin + state = @inferred SatVectorPLLAndDLL( + init_carrier_doppler = 500.0Hz, + init_code_doppler = 100.0Hz, + ) + + @test state.init_carrier_doppler == 500.0Hz + @test state.init_code_doppler == 100.0Hz + @test state.carrier_loop_filter == ThirdOrderAssistedBilinearLF() + @test state.code_loop_filter == SecondOrderBilinearLF() + @test state.carrier_loop_filter_bandwidth == 18.0Hz + @test state.code_loop_filter_bandwidth == 1.0Hz + @test state.code_discr_acc == (0, 0.0) + @test state.code_freq_update == 0.0Hz + @test state.carrier_discr_acc == (0, 0.0Hz) + @test state.carrier_freq_update == 0.0Hz + @test state.vt_on == false + + # Kwarg-update constructor preserves what isn't overridden. + updated = @inferred SatVectorPLLAndDLL( + state; + code_discr_acc = (2, 0.5), + carrier_freq_update = 3.0Hz, + vt_on = true, + ) + @test updated.code_discr_acc == (2, 0.5) + @test updated.carrier_freq_update == 3.0Hz + @test updated.vt_on == true + @test updated.init_carrier_doppler == 500.0Hz + @test updated.carrier_loop_filter_bandwidth == 18.0Hz +end + +@testset "Vector PLL and DLL estimator seeding" begin + gpsl1 = GPSL1CA() + + # Auto bandwidths resolve from the driver signal at seeding time, sized + # exactly like the conventional estimator (the scalar fallback loop). + estimator = VectorPLLAndDLL() + sat = TrackedSat(gpsl1, 1, 0.5, 100.0Hz; doppler_estimator = estimator) + state = get_doppler_estimator_state(sat) + @test state isa SatVectorPLLAndDLL + @test state.carrier_loop_filter_bandwidth == 18.0Hz + @test state.code_loop_filter_bandwidth == 1.0Hz + @test state.init_carrier_doppler == 100.0Hz + @test state.vt_on == false + + # Explicit bandwidths override the auto-sizing; the kwarg-update + # constructor tweaks them afterwards. + explicit = VectorPLLAndDLL(; + carrier_loop_filter_bandwidth = 12.0Hz, + code_loop_filter_bandwidth = 1.0Hz, + ) + explicit_state = @inferred init_estimator_state(explicit, sat) + @test explicit_state.carrier_loop_filter_bandwidth == 12.0Hz + @test explicit_state.code_loop_filter_bandwidth == 1.0Hz + bumped = VectorPLLAndDLL(explicit; carrier_loop_filter_bandwidth = 15.0Hz) + @test bumped.carrier_loop_filter_bandwidth == 15.0Hz + @test bumped.code_loop_filter_bandwidth == 1.0Hz +end + +@testset "Scalar fallback matches the conventional assisted PLL and DLL" begin + sampling_frequency = 5e6Hz + gpsl1 = GPSL1CA() + carrier_doppler = 100.0Hz + prn = 1 + code_phase = 0.5 + num_samples = 5000 + accumulators = (1000.0 + 10im, 2000.0 + 20im, 750.0 + 10im) + + # With vt_on = false (the initial state), the vector estimator must + # reproduce the conventional assisted estimator's Doppler update + # exactly — both auto-size their bandwidths identically. + vector_estimator = VectorPLLAndDLL() + conventional_estimator = ConventionalAssistedPLLAndDLL() + + results = map((vector_estimator, conventional_estimator)) do doppler_estimator + sat = TrackedSat(gpsl1, prn, code_phase, carrier_doppler; doppler_estimator) + sat = _with_full_integration(sat, accumulators, num_samples) + track_state = TrackState(gpsl1, sat; doppler_estimator) + new_track_state = @inferred estimate_dopplers_and_filter_prompt( + track_state, + _meas_l1(sampling_frequency), + ) + (get_carrier_doppler(new_track_state, prn), get_code_doppler(new_track_state, prn)) + end + @test results[1] == results[2] + + # Nothing is accumulated in the scalar fallback, but the scalar DLL + # output is recorded as the code_freq_update. + vector_sat = TrackedSat( + gpsl1, + prn, + code_phase, + carrier_doppler; + doppler_estimator = vector_estimator, + ) + vector_sat = _with_full_integration(vector_sat, accumulators, num_samples) + track_state = TrackState(gpsl1, vector_sat; doppler_estimator = vector_estimator) + new_track_state = + estimate_dopplers_and_filter_prompt(track_state, _meas_l1(sampling_frequency)) + state = get_doppler_estimator_state(get_sat_state(new_track_state, prn)) + @test state.code_discr_acc == (0, 0.0) + @test state.carrier_discr_acc == (0, 0.0Hz) + @test state.code_freq_update != 0.0Hz + @test state.carrier_freq_update == 0.0Hz +end + +@testset "Vector loop closure applies the NCO corrections" begin + sampling_frequency = 5e6Hz + gpsl1 = GPSL1CA() + carrier_doppler = 100.0Hz + prn = 1 + code_phase = 0.5 + num_samples = 5000 + accumulators = (1000.0 + 10im, 2000.0 + 20im, 750.0 + 10im) + nav_carrier_freq_update = 5.0Hz + nav_code_freq_update = -0.25Hz + + doppler_estimator = VectorPLLAndDLL() + sat = TrackedSat(gpsl1, prn, code_phase, carrier_doppler; doppler_estimator) + init_code_doppler = get_code_doppler(sat) + sat = _with_full_integration(sat, accumulators, num_samples) + track_state = TrackState(gpsl1, sat; doppler_estimator) + set_vt_on!(track_state, true, (prn,)) + set_carrier_freq_updates!(track_state, dictionary((prn => nav_carrier_freq_update,))) + set_code_freq_updates!(track_state, dictionary((prn => nav_code_freq_update,))) + + new_track_state = @inferred estimate_dopplers_and_filter_prompt( + track_state, + _meas_l1(sampling_frequency), + ) + state = get_doppler_estimator_state(get_sat_state(new_track_state, prn)) + + # Expected values, computed on the normalized correlator (the post-corr + # filter is an identity for the first prompt). + normalized_correlator = update_accumulator( + get_default_correlator(gpsl1), + SVector(accumulators...) ./ num_samples, + ) + integration_time = num_samples / sampling_frequency + pll_discriminator = pll_disc(gpsl1, normalized_correlator) + dll_discriminator = + dll_disc(gpsl1, normalized_correlator, init_code_doppler, sampling_frequency) + # The FLL branch is driven by the navigation filter's carrier update. + expected_carrier_freq_update, _ = filter_loop( + ThirdOrderAssistedBilinearLF(), + (pll_discriminator, nav_carrier_freq_update), + integration_time, + 18.0Hz, + ) + + @test get_carrier_doppler(new_track_state, prn) == + carrier_doppler + expected_carrier_freq_update + # The code Doppler follows the navigation filter's update directly + # (plus carrier aiding) — the code loop filter is bypassed. + @test get_code_doppler(new_track_state, prn) == + init_code_doppler + + nav_code_freq_update + + expected_carrier_freq_update * get_code_center_frequency_ratio(gpsl1) + + # Discriminator outputs are accumulated for the navigation filter; the + # FLL discriminator is zero here since there is no previous prompt. + @test state.code_discr_acc == (1, dll_discriminator) + @test state.carrier_discr_acc == (1, 0.0Hz) + # The mean accessors divide sum by count (one sample here). + @test mean_code_discriminator(state) == dll_discriminator + @test mean_carrier_discriminator(state) == 0.0Hz + # The navigation filter's corrections survive the update untouched. + @test state.code_freq_update == nav_code_freq_update + @test state.carrier_freq_update == nav_carrier_freq_update + + # The accumulator-reset functions bring the accumulators back to zero. + reset_code_discr_acc!(new_track_state) + reset_carrier_discr_acc!(new_track_state) + state = get_doppler_estimator_state(get_sat_state(new_track_state, prn)) + @test state.code_discr_acc == (0, 0.0) + @test state.carrier_discr_acc == (0, 0.0Hz) + # With count == 0 the mean accessors return `nothing`. + @test mean_code_discriminator(state) === nothing + @test mean_carrier_discriminator(state) === nothing +end + +@testset "Vector tracking state management" begin + gpsl1 = GPSL1CA() + doppler_estimator = VectorPLLAndDLL() + sats = [ + TrackedSat(gpsl1, 1, 0.5, 100.0Hz; doppler_estimator), + TrackedSat(gpsl1, 2, 0.25, 200.0Hz; doppler_estimator), + ] + track_state = TrackState(gpsl1, sats; doppler_estimator) + _state(prn) = get_doppler_estimator_state(get_sat_state(track_state, prn)) + + # PRN 1 in lock joins the vector loop; PRN 2 keeps the scalar fallback. + update_vt_states!(track_state, (1,)) + @test _state(1).vt_on == true + @test _state(2).vt_on == false + + # Promotion is one-directional: PRN 1 dropping out of the lock set does + # NOT demote it — the navigation filter keeps steering it. + update_vt_states!(track_state, ()) + @test _state(1).vt_on == true + @test _state(2).vt_on == false + + # PRN 2 joins once in lock; PRN 1 stays in. + update_vt_states!(track_state, (1, 2)) + @test _state(1).vt_on == true + @test _state(2).vt_on == true + + # Manual demotion via set_vt_on!. + set_vt_on!(track_state, false, (1,)) + @test _state(1).vt_on == false + @test _state(2).vt_on == true + + # Per-PRN NCO corrections only reach satellites in the vector loop. + set_code_freq_updates!(track_state, dictionary((1 => 1.0Hz, 2 => 2.0Hz))) + set_carrier_freq_updates!(track_state, dictionary((1 => 10.0Hz, 2 => 20.0Hz))) + @test _state(1).code_freq_update == 0.0Hz + @test _state(1).carrier_freq_update == 0.0Hz + @test _state(2).code_freq_update == 2.0Hz + @test _state(2).carrier_freq_update == 20.0Hz +end + +@testset "Group-scoped vector tracking state management" begin + # Two constellations sharing a PRN number: the group-scoped managers must + # address exactly one group — a PRN alone is ambiguous across groups. + gpsl1 = GPSL1CA() + galileo_e1b = GalileoE1B() + doppler_estimator = VectorPLLAndDLL() + track_state = TrackState(; + signals = (gps = (gpsl1,), galileo = (galileo_e1b,)), + doppler_estimator, + ) + add_satellite!( + track_state; + prn = 5, + group = :gps, + code_phase = 0.5, + carrier_doppler = 100.0Hz, + ) + add_satellite!( + track_state; + prn = 5, + group = :galileo, + code_phase = 0.25, + carrier_doppler = 200.0Hz, + ) + _state(group) = get_doppler_estimator_state(get_sat_state(track_state, group, 5)) + + # Promotion only reaches the addressed group. + update_vt_states!(track_state, :gps, (5,)) + @test _state(:gps).vt_on == true + @test _state(:galileo).vt_on == false + + # Group-scoped NCO corrections: the other group's dictionary is never + # consulted (no KeyError for its vt-on sats) and its state is untouched. + update_vt_states!(track_state, :galileo, (5,)) + set_code_freq_updates!(track_state, :gps, dictionary((5 => 1.0Hz,))) + set_carrier_freq_updates!(track_state, :gps, dictionary((5 => 10.0Hz,))) + @test _state(:gps).code_freq_update == 1.0Hz + @test _state(:gps).carrier_freq_update == 10.0Hz + @test _state(:galileo).code_freq_update == 0.0Hz + @test _state(:galileo).carrier_freq_update == 0.0Hz + + # Membership changes are group-scoped too. + set_vt_on!(track_state, :gps, false, (5,)) + @test _state(:gps).vt_on == false + @test _state(:galileo).vt_on == true + + # Promoting the same PRN in one group must not touch the other group's + # membership. + set_vt_on!(track_state, :galileo, false, (5,)) + update_vt_states!(track_state, :gps, (5,)) + @test _state(:gps).vt_on == true + @test _state(:galileo).vt_on == false +end + +@testset "reset_loop_filters! preserves VT flags and zeroes corrections" begin + gpsl1 = GPSL1CA() + doppler_estimator = VectorPLLAndDLL() + sat = TrackedSat(gpsl1, 1, 0.5, 100.0Hz; doppler_estimator) + dirty_state = SatVectorPLLAndDLL( + get_doppler_estimator_state(sat); + code_discr_acc = (3, 0.7), + carrier_discr_acc = (3, 2.0Hz), + code_freq_update = 0.5Hz, + carrier_freq_update = 4.0Hz, + carrier_loop_filter_bandwidth = 12.0Hz, + vt_on = true, + ) + track_state = TrackState(gpsl1, _with_state(sat, dirty_state); doppler_estimator) + reset_loop_filters!(track_state, 1) + state = get_doppler_estimator_state(get_sat_state(track_state, 1)) + @test state.code_discr_acc == (0, 0.0) + @test state.carrier_discr_acc == (0, 0.0Hz) + @test state.code_freq_update == 0.0Hz + @test state.carrier_freq_update == 0.0Hz + # Per-sat bandwidth override and the vt_on flag survive the reset. + @test state.carrier_loop_filter_bandwidth == 12.0Hz + @test state.vt_on == true + # Init Dopplers are re-seeded from the sat's current Dopplers. + @test state.init_carrier_doppler == 100.0Hz +end + +end