sidereon_core/precise_positioning/

fixed.rs

//! Static integer-fixed PPP solve.
//!
//! This leaf owns the fixed-ambiguity orchestration: the LAMBDA integer search
//! from a float solution, the ambiguity-conditioned multi-epoch re-solve, the
//! post-fit residual rows, and the cycle/metre ambiguity conversions. The shared
//! measurement model lives in [`super::model`], the dense normal-equation kernel
//! in [`super::normal`], and the row staging / shared scalar helpers in [`super`].

use std::collections::{BTreeMap, BTreeSet};

use crate::ambiguity::AmbiguityId;
use crate::astro::math::vec3;
use crate::estimation::recipe::{EstimationRecipe, NormalRecipe};
use crate::estimation::substrate::ambiguity::resolve_integer_lattice;
use crate::observables::ObservableEphemerisSource;

use super::normal::{
    ambiguity_covariance_from_normal, clock_eliminated_normal_equations, ppp_position_covariance,
    solve_normal_equations, PppNormalLayout,
};
use super::rows::{build_rows, residual_rows, AmbiguityBinding, PppRowError};
use super::temporal::{estimate_temporal_correlation, temporal_position_covariance};
use super::{
    apply_elevation_cutoff, estimates_tropo_gradients, estimates_ztd, max_abs,
    residual_ionosphere_unknown_count, rms, state_from_solution, tropo_gradient_unknown_count,
    validate_fixed_solve_boundary, weighted_rms, ztd_unknown_count, AmbiguitySearch,
    FixedIntegerMetadata, FixedSolution, FixedSolveConfig, FixedSolveError, FloatEpoch,
    FloatSolution, FloatSolveError, FloatSolveOptions, FloatState, FloatStatus, IntegerStatus,
    ModelContext, TroposphereOptions,
};

/// Search integer ambiguities from an existing float PPP solution and re-solve
/// position/clocks with those ambiguities held fixed.
pub fn solve_fixed_from_float(
    source: &dyn ObservableEphemerisSource,
    epochs: &[FloatEpoch],
    float_solution: FloatSolution,
    config: FixedSolveConfig,
) -> Result<FixedSolution, FixedSolveError> {
    validate_fixed_solve_boundary(epochs, &float_solution, &config)?;
    use crate::estimation::recipe::StrategyId;
    use crate::estimation::strategies::{
        estimate, EstimateError, EstimateInput, EstimateOptions, EstimateOutput,
    };
    match estimate(
        EstimateInput::PppFixed {
            source,
            epochs,
            float_solution,
            config,
        },
        EstimateOptions::new(StrategyId::ppp_reference()),
    ) {
        Ok(EstimateOutput::PppFixed(solution)) => Ok(*solution),
        Err(EstimateError::PppFixed(error)) => Err(error),
        Ok(_) | Err(_) => {
            unreachable!(
                "the PPP reference strategy yields a PPP fixed solution or a PPP fixed error"
            )
        }
    }
}

/// Drive the integer-fixed PPP re-solve from a resolved [`EstimationRecipe`]: the
/// shared per-technique implementation that
/// [`crate::estimation::strategies::estimate`] dispatches to. The recipe's
/// [`NormalRecipe`] reaches the solve seam through [`ModelContext::normal`]; for
/// the PPP reference recipe (`NormalRecipe::PppDenseLastTie`) this is
/// bit-identical to the legacy path.
pub(crate) fn run_fixed_from_float(
    recipe: &EstimationRecipe,
    source: &dyn ObservableEphemerisSource,
    epochs: &[FloatEpoch],
    float_solution: FloatSolution,
    config: FixedSolveConfig,
) -> Result<FixedSolution, FixedSolveError> {
    validate_fixed_solve_boundary(epochs, &float_solution, &config)?;
    let initial_state = fixed_state_from_float(&float_solution);
    let filtered_epochs;
    let solve_epochs = if let Some(cutoff_deg) = config.elevation_cutoff_deg {
        filtered_epochs = apply_elevation_cutoff(
            source,
            epochs,
            &initial_state,
            cutoff_deg,
            config.tropo,
            config.estimate_residual_ionosphere,
        )
        .map_err(FixedSolveError::Float)?;
        filtered_epochs.as_slice()
    } else {
        epochs
    };
    let active_order;
    let search_order = if config.elevation_cutoff_deg.is_some() {
        active_order = active_ambiguity_ids(solve_epochs);
        Some(active_order.as_slice())
    } else {
        None
    };
    let fixed_meta =
        search_integer_ambiguities(source, solve_epochs, &float_solution, &config, search_order)?;
    let fixed_m = fixed_ambiguities_m(
        &fixed_meta.fixed_cycles,
        &config.ambiguity.wavelengths_m,
        &config.ambiguity.offsets_m,
    )?;
    let ctx = ModelContext {
        source,
        weights: config.weights,
        tropo: config.tropo,
        corrections: &config.corrections,
        normal: recipe.normal,
        estimate_residual_ionosphere: config.estimate_residual_ionosphere,
    };
    let resolve = iterate_fixed_multi(ctx, solve_epochs, &fixed_m, initial_state, config.opts, 1)?;
    finalize_fixed_multi(
        ctx,
        solve_epochs,
        fixed_meta,
        fixed_m,
        float_solution,
        resolve,
    )
}

struct FixedSearchResult {
    order: Vec<AmbiguityId>,
    fixed_cycles: BTreeMap<String, i64>,
    integer: FixedIntegerMetadata,
}

/// Converged state from the ambiguity-conditioned re-solve, carried from
/// [`iterate_fixed_multi`] into [`finalize_fixed_multi`].
struct FixedResolve {
    state: FloatState,
    iterations: usize,
    converged: bool,
    status: FloatStatus,
}

impl From<FloatSolveError> for FixedSolveError {
    fn from(value: FloatSolveError) -> Self {
        Self::Float(value)
    }
}

fn search_integer_ambiguities(
    source: &dyn ObservableEphemerisSource,
    epochs: &[FloatEpoch],
    float_solution: &FloatSolution,
    config: &FixedSolveConfig,
    active_order: Option<&[AmbiguityId]>,
) -> Result<FixedSearchResult, FixedSolveError> {
    let order: Vec<AmbiguityId> = active_order.map_or_else(
        || {
            float_solution
                .used_sats
                .iter()
                .map(|sat| AmbiguityId::new(sat.clone()))
                .collect()
        },
        |order| order.to_vec(),
    );
    let covariance_cycles =
        ambiguity_covariance_cycles(source, epochs, &order, float_solution, config)?;
    let float_cycles = float_ambiguities_cycles(
        float_solution,
        &config.ambiguity.wavelengths_m,
        &config.ambiguity.offsets_m,
    )?;
    let floats: Vec<f64> = order
        .iter()
        .map(|id| float_cycles.get(id.as_str()).copied().unwrap())
        .collect();
    let result = resolve_integer_lattice(
        &floats,
        &covariance_cycles,
        config.ambiguity.ratio_threshold,
    )
    .map_err(FixedSolveError::Integer)?;
    let fixed_cycles = order
        .iter()
        .map(|id| id.as_str().to_string())
        .zip(result.fixed.iter().copied())
        .collect::<BTreeMap<_, _>>();
    let search_order: Vec<String> = order.iter().map(|id| id.as_str().to_string()).collect();
    Ok(FixedSearchResult {
        order,
        fixed_cycles,
        integer: FixedIntegerMetadata {
            integer_status: if result.fixed_status {
                IntegerStatus::Fixed
            } else {
                IntegerStatus::NotFixed
            },
            integer_ratio: result.ratio,
            integer_best_score: result.best_score,
            integer_second_best_score: result.second_best_score,
            integer_candidates: result.candidates_evaluated,
            ambiguity_search: AmbiguitySearch {
                order: search_order,
                float_cycles,
                covariance_cycles: result.covariance,
                covariance_inverse_cycles: result.covariance_inverse,
            },
        },
    })
}

fn active_ambiguity_ids(epochs: &[FloatEpoch]) -> Vec<AmbiguityId> {
    epochs
        .iter()
        .flat_map(|e| {
            e.observations
                .iter()
                .map(|o| AmbiguityId::new(o.ambiguity_id.clone()))
        })
        .collect::<BTreeSet<_>>()
        .into_iter()
        .collect()
}

fn iterate_fixed_multi(
    ctx: ModelContext,
    epochs: &[FloatEpoch],
    fixed_m: &BTreeMap<String, f64>,
    state: FloatState,
    opts: FloatSolveOptions,
    iter: usize,
) -> Result<FixedResolve, FixedSolveError> {
    let mut current = state;
    let mut iteration = iter;
    let max_iterations = opts.max_iterations;

    loop {
        let binding = AmbiguityBinding::Held { values: fixed_m };
        let rows = build_rows(ctx, epochs, &binding, &current).map_err(PppRowError::into_fixed)?;
        let layout = PppNormalLayout::new(
            epochs.len(),
            ztd_unknown_count(ctx.tropo),
            tropo_gradient_unknown_count(ctx.tropo),
            residual_ionosphere_unknown_count(ctx.estimate_residual_ionosphere, fixed_m.len()),
            0,
        );
        let dx = solve_normal_equations(&rows, layout, ctx.normal)?;
        let next = apply_fixed_multi_delta(
            &current,
            epochs.len(),
            fixed_m,
            &dx,
            ctx.tropo,
            ctx.estimate_residual_ionosphere,
        );
        let (pos_step, clock_step, ztd_step, gradient_step) = fixed_multi_step_norms(
            &dx,
            ctx.tropo,
            ctx.estimate_residual_ionosphere,
            fixed_m.len(),
        );

        if pos_step <= opts.position_tolerance_m
            && clock_step <= opts.clock_tolerance_m
            && ztd_step <= opts.ztd_tolerance_m
            && gradient_step <= opts.ztd_tolerance_m
        {
            return Ok(FixedResolve {
                state: next,
                iterations: iteration,
                converged: true,
                status: FloatStatus::StateTolerance,
            });
        }

        if iteration >= max_iterations {
            return Ok(FixedResolve {
                state: next,
                iterations: iteration,
                converged: false,
                status: FloatStatus::MaxIterations,
            });
        }

        current = next;
        iteration += 1;
    }
}

fn apply_fixed_multi_delta(
    state: &FloatState,
    n_epochs: usize,
    fixed_m: &BTreeMap<String, f64>,
    dx: &[f64],
    tropo: TroposphereOptions,
    estimate_residual_ionosphere: bool,
) -> FloatState {
    let mut idx = 3;
    let clock_deltas = &dx[idx..idx + n_epochs];
    idx += n_epochs;
    let ztd_delta = if estimates_ztd(tropo) {
        let v = dx[idx];
        idx += 1;
        v
    } else {
        0.0
    };
    let (tropo_gradient_north_delta, tropo_gradient_east_delta) =
        if estimates_tropo_gradients(tropo) {
            let north = dx[idx];
            let east = dx[idx + 1];
            idx += 2;
            (north, east)
        } else {
            (0.0, 0.0)
        };
    let mut residual_ionosphere_m = BTreeMap::new();
    if estimate_residual_ionosphere {
        let ionosphere_deltas = &dx[idx..idx + fixed_m.len()];
        for ((id, _), delta) in fixed_m.iter().zip(ionosphere_deltas) {
            let prior = state.residual_ionosphere_m.get(id).copied().unwrap_or(0.0);
            residual_ionosphere_m.insert(id.clone(), prior + delta);
        }
    }
    let clocks_m = state
        .clocks_m
        .iter()
        .zip(clock_deltas)
        .map(|(clock, delta)| clock + delta)
        .collect();
    FloatState {
        position_m: [
            state.position_m[0] + dx[0],
            state.position_m[1] + dx[1],
            state.position_m[2] + dx[2],
        ],
        clocks_m,
        ambiguities_m: BTreeMap::new(),
        ztd_m: state.ztd_m + ztd_delta,
        tropo_gradient_north_m: state.tropo_gradient_north_m + tropo_gradient_north_delta,
        tropo_gradient_east_m: state.tropo_gradient_east_m + tropo_gradient_east_delta,
        residual_ionosphere_m,
    }
}

fn fixed_multi_step_norms(
    dx: &[f64],
    tropo: TroposphereOptions,
    estimate_residual_ionosphere: bool,
    n_residual_ionosphere: usize,
) -> (f64, f64, f64, f64) {
    let pos = vec3::norm3([dx[0], dx[1], dx[2]]);
    let n_ztd = ztd_unknown_count(tropo);
    let n_gradients = tropo_gradient_unknown_count(tropo);
    let n_ionosphere =
        residual_ionosphere_unknown_count(estimate_residual_ionosphere, n_residual_ionosphere);
    let n_clocks = dx.len() - 3 - n_ztd - n_gradients - n_ionosphere;
    let clock = max_abs(&dx[3..3 + n_clocks]);
    let mut idx = 3 + n_clocks;
    let ztd = if estimates_ztd(tropo) {
        let v = dx[idx].abs();
        idx += 1;
        v
    } else {
        0.0
    };
    let gradient = if estimates_tropo_gradients(tropo) {
        let v = max_abs(&dx[idx..idx + 2]);
        idx += 2;
        v
    } else {
        0.0
    };
    let ionosphere = if estimate_residual_ionosphere {
        max_abs(&dx[idx..idx + n_residual_ionosphere])
    } else {
        0.0
    };
    (pos, clock, ztd, gradient.max(ionosphere))
}

fn finalize_fixed_multi(
    ctx: ModelContext,
    epochs: &[FloatEpoch],
    search: FixedSearchResult,
    fixed_m: BTreeMap<String, f64>,
    float_solution: FloatSolution,
    resolve: FixedResolve,
) -> Result<FixedSolution, FixedSolveError> {
    let FixedResolve {
        state,
        iterations,
        converged,
        status,
    } = resolve;
    let residuals =
        residual_rows(ctx, epochs, &fixed_m, &state).map_err(PppRowError::into_fixed)?;
    let binding = AmbiguityBinding::Held { values: &fixed_m };
    let rows = build_rows(ctx, epochs, &binding, &state).map_err(PppRowError::into_fixed)?;
    let covariance = ppp_position_covariance(
        &rows,
        PppNormalLayout::new(
            epochs.len(),
            ztd_unknown_count(ctx.tropo),
            tropo_gradient_unknown_count(ctx.tropo),
            residual_ionosphere_unknown_count(ctx.estimate_residual_ionosphere, fixed_m.len()),
            0,
        ),
        state.position_m,
    )?;
    let code: Vec<f64> = residuals.iter().map(|r| r.code_m).collect();
    let phase: Vec<f64> = residuals.iter().map(|r| r.phase_m).collect();
    let temporal_correlation = estimate_temporal_correlation(&residuals, epochs);
    let (temporal_position_covariance, temporal_position_covariance_scale_factor) =
        temporal_position_covariance(
            covariance.formal,
            covariance.posterior_variance_factor,
            temporal_correlation,
        );
    Ok(FixedSolution {
        position_m: state.position_m,
        position_covariance: covariance.scaled,
        formal_position_covariance: covariance.formal,
        posterior_variance_factor: covariance.posterior_variance_factor,
        position_covariance_scale_factor: covariance.covariance_scale_factor,
        temporal_position_covariance,
        temporal_position_covariance_scale_factor,
        temporal_correlation,
        epoch_clocks_m: state.clocks_m,
        fixed_ambiguities_cycles: search.fixed_cycles,
        fixed_ambiguities_m: fixed_m,
        residual_ionosphere_m: if ctx.estimate_residual_ionosphere {
            state.residual_ionosphere_m
        } else {
            BTreeMap::new()
        },
        ztd_residual_m: if estimates_ztd(ctx.tropo) {
            Some(state.ztd_m)
        } else {
            None
        },
        tropo_gradient_north_m: if estimates_tropo_gradients(ctx.tropo) {
            Some(state.tropo_gradient_north_m)
        } else {
            None
        },
        tropo_gradient_east_m: if estimates_tropo_gradients(ctx.tropo) {
            Some(state.tropo_gradient_east_m)
        } else {
            None
        },
        tropo_gradient_covariance_m2: covariance.tropo_gradient_scaled_m2,
        formal_tropo_gradient_covariance_m2: covariance.tropo_gradient_formal_m2,
        float_solution,
        residuals_m: residuals.clone(),
        used_sats: search
            .order
            .into_iter()
            .map(AmbiguityId::into_string)
            .collect(),
        iterations,
        converged,
        status,
        code_rms_m: rms(&code),
        phase_rms_m: rms(&phase),
        weighted_rms_m: weighted_rms(&residuals, ctx.weights),
        integer: search.integer,
    })
}

fn fixed_state_from_float(solution: &FloatSolution) -> FloatState {
    FloatState {
        position_m: solution.position_m,
        clocks_m: solution.epoch_clocks_m.clone(),
        ambiguities_m: BTreeMap::new(),
        ztd_m: solution.ztd_residual_m.unwrap_or(0.0),
        tropo_gradient_north_m: solution.tropo_gradient_north_m.unwrap_or(0.0),
        tropo_gradient_east_m: solution.tropo_gradient_east_m.unwrap_or(0.0),
        residual_ionosphere_m: solution.residual_ionosphere_m.clone(),
    }
}

fn float_ambiguities_cycles(
    solution: &FloatSolution,
    wavelengths_m: &BTreeMap<String, f64>,
    offsets_m: &BTreeMap<String, f64>,
) -> Result<BTreeMap<String, f64>, FixedSolveError> {
    let mut out = BTreeMap::new();
    for sat in &solution.used_sats {
        let wavelength = wavelengths_m
            .get(sat)
            .copied()
            .ok_or_else(|| FixedSolveError::MissingWavelength(sat.clone()))?;
        let offset = offsets_m
            .get(sat)
            .copied()
            .ok_or_else(|| FixedSolveError::MissingOffset(sat.clone()))?;
        let ambiguity_m = solution.ambiguities_m.get(sat).copied().ok_or_else(|| {
            FixedSolveError::Float(FloatSolveError::MissingAmbiguity(sat.clone()))
        })?;
        out.insert(sat.clone(), (ambiguity_m - offset) / wavelength);
    }
    Ok(out)
}

fn fixed_ambiguities_m(
    fixed_cycles: &BTreeMap<String, i64>,
    wavelengths_m: &BTreeMap<String, f64>,
    offsets_m: &BTreeMap<String, f64>,
) -> Result<BTreeMap<String, f64>, FixedSolveError> {
    let mut out = BTreeMap::new();
    for (sat, cycles) in fixed_cycles {
        let wavelength = wavelengths_m
            .get(sat)
            .copied()
            .ok_or_else(|| FixedSolveError::MissingWavelength(sat.clone()))?;
        let offset = offsets_m
            .get(sat)
            .copied()
            .ok_or_else(|| FixedSolveError::MissingOffset(sat.clone()))?;
        out.insert(sat.clone(), offset + *cycles as f64 * wavelength);
    }
    Ok(out)
}

fn ambiguity_covariance_cycles(
    source: &dyn ObservableEphemerisSource,
    epochs: &[FloatEpoch],
    ambiguity_ids: &[AmbiguityId],
    float_solution: &FloatSolution,
    config: &FixedSolveConfig,
) -> Result<Vec<Vec<f64>>, FixedSolveError> {
    let state = state_from_solution(float_solution, &FloatState::default_for_epochs(epochs));
    let layout = PppNormalLayout::new(
        epochs.len(),
        ztd_unknown_count(config.tropo),
        tropo_gradient_unknown_count(config.tropo),
        residual_ionosphere_unknown_count(config.estimate_residual_ionosphere, ambiguity_ids.len()),
        ambiguity_ids.len(),
    );
    let start = layout.reduced_ambiguity_offset();
    let ctx = ModelContext {
        source,
        weights: config.weights,
        tropo: config.tropo,
        corrections: &config.corrections,
        // Covariance assembly uses the const last-tie assembler directly; the
        // recipe field is the PPP reference and unused on this path.
        normal: NormalRecipe::PppDenseLastTie,
        estimate_residual_ionosphere: config.estimate_residual_ionosphere,
    };
    let binding = AmbiguityBinding::Estimated {
        ids: ambiguity_ids,
        values: &state.ambiguities_m,
    };
    let rows = build_rows(ctx, epochs, &binding, &state)
        .map_err(|e| FixedSolveError::from(e.into_float()))?;
    let (normal, _rhs) = clock_eliminated_normal_equations(&rows, layout)?;
    let covariance_m = ambiguity_covariance_from_normal(&normal, start, ambiguity_ids.len())?;
    let mut covariance_cycles = vec![vec![0.0; ambiguity_ids.len()]; ambiguity_ids.len()];
    for i in 0..ambiguity_ids.len() {
        let lambda_i = config
            .ambiguity
            .wavelengths_m
            .get(ambiguity_ids[i].as_str())
            .copied()
            .ok_or_else(|| {
                FixedSolveError::MissingWavelength(ambiguity_ids[i].as_str().to_string())
            })?;
        for j in 0..ambiguity_ids.len() {
            let lambda_j = config
                .ambiguity
                .wavelengths_m
                .get(ambiguity_ids[j].as_str())
                .copied()
                .ok_or_else(|| {
                    FixedSolveError::MissingWavelength(ambiguity_ids[j].as_str().to_string())
                })?;
            covariance_cycles[i][j] = covariance_m[i][j] / (lambda_i * lambda_j);
        }
    }
    Ok(covariance_cycles)
}