From 18d9e50e584bc3b44f5668f531d5fa7833b7d3b4 Mon Sep 17 00:00:00 2001 From: gwbres Date: Fri, 1 Dec 2023 18:22:13 +0100 Subject: [PATCH] Develop (#1) * sv relativistic effects compensation * relativistic effect compensation * update doc * working on residual validation * lib update * make sure we still have enough candidates * test signal against mode compliance * run linter --------- Signed-off-by: Guillaume W. Bres --- Cargo.toml | 3 +- README.md | 99 +++++++++++++++----------- doc/config.md | 10 +++ doc/lsqspp.md | 4 ++ doc/ppp.md | 4 ++ doc/spp.md | 32 +++++++++ src/candidate.rs | 47 ++++++++----- src/cfg.rs | 115 ++++++++++++++++++++----------- src/solutions.rs | 14 +++- src/solver.rs | 176 ++++++++++++++++++++++++++++++++++++++--------- 10 files changed, 367 insertions(+), 137 deletions(-) create mode 100644 doc/config.md create mode 100644 doc/lsqspp.md create mode 100644 doc/ppp.md create mode 100644 doc/spp.md diff --git a/Cargo.toml b/Cargo.toml index be794d9..5ca1f96 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "gnss-rtk" -version = "0.3.0" +version = "0.3.1" license = "MIT OR Apache-2.0" authors = ["Guillaume W. Bres "] description = "GNSS position solver" @@ -15,6 +15,7 @@ readme = "README.md" log = "0.4" thiserror = "1" map_3d = "0.1.5" +itertools = "0.12.0" nalgebra = "0.32.3" nyx-space = "=2.0.0-beta.0" gnss-rs = { version = "2.1.2", features = ["serde"] } diff --git a/README.md b/README.md index 1111a84..c998152 100644 --- a/README.md +++ b/README.md @@ -8,46 +8,76 @@ GNSS-RTK Precise position solver :crab: -Solving method -============== +Notes on this ecosystem +======================= -The solver uses linear algebra implemented in the `nalgebra` library. +We use `nalgebra` in the solving process (Matrix, Vectors..). +We use `nyx` for ephemerides, orbital calculations, navigation filters, etc.. +`Hifitime` is at the very basis of this work and allows pretty much everything here. +[The RINEX Wiki](https://github.com/georust/rinex/wiki) describes extensive application of this framework. +[The RNX2CGGTTS and its Wiki](https://github.com/georust/rinex/wiki) is another interesting application of this framework. -The current solver is straightforward and does not require initialization cycles. -We can resolve a PVT for every input as long as the criterias for the current setup are met. +PVT Solutions +============= -Some criterias are fixed by physics, others are customized and adjusted in the `Cfg` structure. +The objective is to resolve a PVT solution, implemented in the form of a `prelude::PVTSolution`, +ideally the most precise as possible, at a given _Epoch_. -The minimum requirements to form a solution : +Resolving a PVT requires a minimum number of SV (actual measurements) +in sight and within the predefined criterias: -- the minimal number of SV within the criterias are in sight: - - 4 SV in default mode - - 3 SV in fixed altitude mode - - 1 SV in time only mode -- user was able to provide observations that satisfy the resolution strategy -- user was able to interpolate the SV state vector at the required _Epoch_ +- 4 SV in default mode +- 3 SV in fixed altitude mode +- 1 SV in time only mode -PVT Solutions -============= +The predefined criterias are manually set in the configuration file, +refer to its [own documentation](./doc/config.md). This means the criterias can be +loosened or tightened, depending on what you want to achieve. + +Some constraints on the measurements may apply too, as a rule of thumb: + +- `SPP` strategies will only require Pseudo Range and will resolve without Phase observations. +This makes these strategies capapble to deploy on constrained and degraged environment. +For example: old receivers, single constellation in sight.. etc.. + +- `PPP` strategies require not only Pseudo Range but also Phase observations +and the sampling of two different radio frequencies. No solutions will be formed +if this predicate (on top of all the others explained here) do not stand. + +We support several methods (also sometimes refered to as _strategies_) which +will have the solver behave differently, due to their very different nature. +The method is set by the `solver::Mode`. -The solver tries to resolve a Position Velocity Time (PVT) solution of the receiver. -Currently the Velocity term is not evaluated, therefore we only output Positions and Time errors. -Dilution of precisions are estimated and attached to each solution. +Dilution of precisions along other meaningful information are attached to each PVT solution. -Single Point Position (SPP) -=========================== +This solver will eventually be able to resolve along any known GNSS constellation, +including a mixed pool of spacecrafts (for example a combination of GAL + GPS), +and also express the PVT solution against any supported GNSS Time system (for example GST). -The solver supports the SPP resolution method. +Strategies and behavior +======================= -SPP can be deployed in degraded conditions where only one carrier signal and/or a unique constellation is in sight. -In such conditions: +We support a couple of strategies, only advanced strategies will give the best results +(most precise solutions): -- you can only hope for a precision of a few meters. -- an interpolation order of 9 makes sense, going above will increase the computation load without any benefit. -- the solver will have to estimate the total bias due to Ionospheric delay. If you can define -these components accurately yourself, you are encouranged to do it (see related API section and following paragraphs) +- [spp](./doc/spp.md) +- [lsqspp](./doc/lsqspp.md) +- [ppp](./doc/ppp.md) -## Atmospherical and Environmental conditions modeling +As a rule of thumb: + +- Non recursive strategies (like [spp](./doc/spp.md)) will generate +a solution as along as enough signal measurements were provided. +- Any physical phenomena can be accounted for in this framework, +on any strategy, even though some will not be meaningful depending on the +configuration setup. +- The GDOP criteria can still be used to reject PVT solutions of non recursive strategies +- Only recursive strategies will take truly use other solver options of the configuration file. + +The current API allows changing the strategy from one Epoch to another and this framework will most likely behave fine. +But note that this has not been tested and is not the typical use of such tool. + +## Atmospherical and Environmental biases This solver is always capable of modelizing all conditions and form a solution. It is important to understand how our API is designed and operate it the best you can to get the best results. @@ -90,16 +120,3 @@ It is important to understand how, when and what to customize depending on your When working with the "cli" application, you can provide an RTKConfiguration in the form of JSON, with `--rtk-cfg`. - -RINEX Post Processing -===================== - -The [rinex-cli application](https://github.com/georust/rinex) is there -to post process RINEX data and resolve PVT solutions from it. - -CGGTTS: PVT and advanced Timing -=============================== - -The [rnx2cggtts application](https://github.com/georust/rinex) uses this -solver to estimate local clock performances and allow the comparison -of two remote clocks. diff --git a/doc/config.md b/doc/config.md new file mode 100644 index 0000000..6ea7808 --- /dev/null +++ b/doc/config.md @@ -0,0 +1,10 @@ +Solver configuration +==================== + +The `Cfg` structure is how to tune and adjust this position solver. +This solver is smart enough to generate more than decent results even if we rely on default settings +(quick test / quick setups). + +Note that we always prefer precision over computation load. +In other words, for each mode the default options are already computation intensive +and can be simplified. diff --git a/doc/lsqspp.md b/doc/lsqspp.md new file mode 100644 index 0000000..34771c2 --- /dev/null +++ b/doc/lsqspp.md @@ -0,0 +1,4 @@ +LSQSPP +====== + +TODO diff --git a/doc/ppp.md b/doc/ppp.md new file mode 100644 index 0000000..b9348de --- /dev/null +++ b/doc/ppp.md @@ -0,0 +1,4 @@ +PPP +=== + +TODO diff --git a/doc/spp.md b/doc/spp.md new file mode 100644 index 0000000..8655af9 --- /dev/null +++ b/doc/spp.md @@ -0,0 +1,32 @@ +Single Point Positioning +======================== + +SPP can be deployed in degraded conditions where only one carrier signal and/or a unique constellation is in sight. + +Measurements requirements +========================= + +`Mode::SPP` has less constraint on the measurements: +you only need to gather the approriate amount of pseudo range measurements. + +Several "bonus" may be unlocked if you provide extra measurements + +* Providing dopplers has the benefit to validate the interpolated SV state vector +* Providing phase measurements will unlock fractional pseudo range estimate *in the future* +* Providing measurements on another signal will unlock the true ionosphere delay to be estimated. + +Expected SPP configration +========================= + +In `SPP` an interpolation of 9 makes sense and high orders will not be meaningful. +You can use that to reduce the computation load. + +Usually, in this mode the user provides measurements from a unique carrier signal. +In this context, only a modeling of the ionosphere impact is feasible. +As in other + +- you can only hope for a precision of a few meters. +- an interpolation order of 9 makes sense, going above will increase the computation load without any benefit. +- the solver will have to estimate the total bias due to Ionospheric delay. If you can define +these components accurately yourself, you are encouranged to do it (see related API section and following paragraphs) + diff --git a/src/candidate.rs b/src/candidate.rs index 745be6d..f2f49c3 100644 --- a/src/candidate.rs +++ b/src/candidate.rs @@ -2,9 +2,13 @@ use gnss::prelude::SV; use hifitime::Unit; +use itertools::Itertools; use log::debug; +use map_3d::deg2rad; + use nyx::cosmic::SPEED_OF_LIGHT; use nyx::linalg::{DVector, MatrixXx4}; +use nyx::md::prelude::Frame; use crate::prelude::{Config, Duration, Epoch, InterpolationResult, Mode, Vector3}; use crate::solutions::{PVTBias, PVTSVData}; @@ -93,6 +97,28 @@ impl Candidate { // .map(|pr| pr.value) .reduce(|k, _| k) } + /* + * Returns true if we have pseudo range observ on two carriers + */ + pub(crate) fn dual_freq_pseudorange(&self) -> bool { + self.code + .iter() + .map(|c| (c.frequency * 10.0) as u16) + .unique() + .count() + > 0 + } + /* + * Returns true if we have phase observ on two carriers + */ + pub(crate) fn dual_freq_phase(&self) -> bool { + self.phase + .iter() + .map(|c| (c.frequency * 10.0) as u16) + .unique() + .count() + > 0 + } /* * apply min SNR mask */ @@ -153,26 +179,12 @@ impl Candidate { }) } */ - /* - * Returns IONOD possibly impacting - pub(crate) fn ionod_model(&self, frequency: f64) -> Option { - self.modeled_ionod - .iter() - .filter_map(|ionod| { - if ionod.frequency == frequency { - Some(ionod.time_delay) - } else { - None - } - }) - .reduce(|k, _| k) - } - */ /* * Computes signal transmission Epoch * returns (t_tx, dt_ttx) * "t_tx": Epoch in given timescale * "dt_ttx": elapsed duration in seconds in given timescale + * "frame": Solid body reference Frame */ pub(crate) fn transmission_time(&self, cfg: &Config) -> Result<(Epoch, f64), Error> { let (t, ts) = (self.t, self.t.time_scale); @@ -241,7 +253,10 @@ impl Candidate { g[(row_index, 2)] = (z0 - sv_z) / rho; g[(row_index, 3)] = 1.0_f64; - let mut models = -clock_corr * SPEED_OF_LIGHT; + let mut models = 0.0_f64; + if cfg.modeling.sv_clock_bias { + models -= clock_corr * SPEED_OF_LIGHT; + } /* * Possible delay compensations diff --git a/src/cfg.rs b/src/cfg.rs index 8530650..56ee519 100644 --- a/src/cfg.rs +++ b/src/cfg.rs @@ -75,7 +75,11 @@ fn default_earth_rot() -> bool { true } -fn default_rel_clock_corr() -> bool { +fn default_relativistic_clock_bias() -> bool { + true +} + +fn default_relativistic_path_range() -> bool { false } @@ -99,6 +103,35 @@ pub struct InternalDelay { pub frequency: f64, } +#[derive(Default, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde", derive(Deserialize))] +pub struct SolverOpts { + /// GDOP Threshold (max. limit) to invalidate ongoing GDOP + pub gdop_threshold: Option, + /// Weight Matrix in LSQ solving process + #[cfg_attr(feature = "serde", serde(default = "default_lsq_weight"))] + pub lsq_weight: Option, +} + +impl SolverOpts { + /* + * form the weight matrix to be used in the solving process + */ + pub(crate) fn lsq_weight_matrix(&self, nb_rows: usize, sv_elev: Vec) -> DMatrix { + let mut mat = DMatrix::identity(sv_elev.len(), sv_elev.len()); + match &self.lsq_weight { + Some(LSQWeight::LSQWeightCovar) => panic!("not implemented yet"), + Some(LSQWeight::LSQWeightMappingFunction(mapf)) => { + for i in 0..sv_elev.len() - 1 { + mat[(i, i)] = mapf.a + mapf.b * ((-sv_elev[i]) / mapf.c).exp(); + } + }, + None => {}, + } + mat + } +} + /// Atmospherical, Physical and Environmental modeling #[derive(Copy, Clone, Debug, PartialEq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] @@ -106,15 +139,17 @@ pub struct Modeling { #[cfg_attr(feature = "serde", serde(default))] pub sv_clock_bias: bool, #[cfg_attr(feature = "serde", serde(default))] + pub sv_total_group_delay: bool, + #[cfg_attr(feature = "serde", serde(default))] + pub relativistic_clock_bias: bool, + #[cfg_attr(feature = "serde", serde(default))] + pub relativistic_path_range: bool, + #[cfg_attr(feature = "serde", serde(default))] pub tropo_delay: bool, #[cfg_attr(feature = "serde", serde(default))] pub iono_delay: bool, #[cfg_attr(feature = "serde", serde(default))] - pub sv_total_group_delay: bool, - #[cfg_attr(feature = "serde", serde(default))] pub earth_rotation: bool, - #[cfg_attr(feature = "serde", serde(default))] - pub relativistic_clock_corr: bool, } impl Default for Modeling { @@ -125,7 +160,8 @@ impl Default for Modeling { tropo_delay: default_tropo(), sv_total_group_delay: default_sv_tgd(), earth_rotation: default_earth_rot(), - relativistic_clock_corr: default_rel_clock_corr(), + relativistic_clock_bias: default_relativistic_clock_bias(), + relativistic_path_range: default_relativistic_path_range(), } } } @@ -135,11 +171,7 @@ impl From for Modeling { let s = Self::default(); match mode { Mode::PPP => { - // TODO: unlock this please - // s.earth_rotation = true; - // TODO : unlock this please - // s.relativistic_clock_corr = true; - // TODO: + // TODO ? }, _ => {}, } @@ -168,9 +200,9 @@ pub struct Config { /// Internal delays #[cfg_attr(feature = "serde", serde(default))] pub int_delay: Vec, - /// Weight Matrix in LSQ solving process - #[cfg_attr(feature = "serde", serde(default = "default_lsq_weight"))] - pub lsq_weight: Option, + /// Solver customization + #[cfg_attr(feature = "serde", serde(default))] + pub solver: SolverOpts, /// Time Reference Delay, as defined by BIPM in /// "GPS Receivers Accurate Time Comparison" : the time delay /// between the GNSS receiver external reference clock and the sampling clock @@ -200,21 +232,24 @@ pub struct Config { } impl Config { - pub fn default(solver: Mode) -> Self { - match solver { + pub fn default(mode: Mode) -> Self { + match mode { Mode::SPP => Self { timescale: default_timescale(), fixed_altitude: None, interp_order: default_interp(), code_smoothing: default_smoothing(), min_sv_sunlight_rate: None, - min_sv_elev: Some(10.0), + min_sv_elev: Some(15.0), min_snr: Some(30.0), modeling: Modeling::default(), max_sv: default_max_sv(), int_delay: Default::default(), externalref_delay: Default::default(), - lsq_weight: None, + solver: SolverOpts { + lsq_weight: None, + gdop_threshold: None, + }, }, Mode::LSQSPP => Self { timescale: default_timescale(), @@ -228,13 +263,16 @@ impl Config { max_sv: default_max_sv(), int_delay: Default::default(), externalref_delay: Default::default(), - lsq_weight: Some(LSQWeight::LSQWeightMappingFunction( - ElevationMappingFunction { - a: 0.03, - b: 0.03, - c: 100.0, - }, - )), + solver: SolverOpts { + gdop_threshold: Some(30.0), + lsq_weight: Some(LSQWeight::LSQWeightMappingFunction( + ElevationMappingFunction { + a: 0.03, + b: 0.03, + c: 100.0, + }, + )), + }, }, Mode::PPP => Self { timescale: default_timescale(), @@ -249,25 +287,18 @@ impl Config { max_sv: default_max_sv(), int_delay: Default::default(), externalref_delay: Default::default(), - lsq_weight: default_lsq_weight(), - }, - } - } - /* - * form the weight matrix to be used in the solving process - */ - pub(crate) fn lsq_weight_matrix(&self, nb_rows: usize, sv_elev: Vec) -> DMatrix { - let mut mat = DMatrix::identity(sv_elev.len(), sv_elev.len()); - match &self.lsq_weight { - Some(LSQWeight::LSQWeightCovar) => panic!("not implemented yet"), - Some(LSQWeight::LSQWeightMappingFunction(mapf)) => { - for i in 0..sv_elev.len() - 1 { - mat[(i, i)] = mapf.a + mapf.b * ((-sv_elev[i]) / mapf.c).exp(); - } + solver: SolverOpts { + gdop_threshold: None, //TODO + lsq_weight: Some(LSQWeight::LSQWeightMappingFunction( + ElevationMappingFunction { + a: 0.03, + b: 0.03, + c: 100.0, + }, + )), + }, }, - None => {}, } - mat } } diff --git a/src/solutions.rs b/src/solutions.rs index 6eb66c3..3c3b8dc 100644 --- a/src/solutions.rs +++ b/src/solutions.rs @@ -167,15 +167,23 @@ impl PVTSolution { pub fn sv(&self) -> Vec { self.sv.keys().copied().collect() } - /// Returns Horizontal Dilution of Precision of this PVT + /// Returns Geometrical Dilution of Precision of Self + pub fn gdop(&self) -> f64 { + (self.estimate.covar[(0, 0)] + + self.estimate.covar[(1, 1)] + + self.estimate.covar[(2, 2)] + + self.estimate.covar[(3, 3)]) + .sqrt() + } + /// Returns Horizontal Dilution of Precision of Self pub fn hdop(&self) -> f64 { (self.estimate.covar[(0, 0)] + self.estimate.covar[(1, 1)]).sqrt() } - /// Returns Vertical Dilution of Precision of this PVT + /// Returns Vertical Dilution of Precision of Self pub fn vdop(&self) -> f64 { self.estimate.covar[(2, 2)].sqrt() } - /// Returns Time Dilution of Precision of this PVT + /// Returns Time Dilution of Precision of Self pub fn tdop(&self) -> f64 { self.estimate.covar[(3, 3)].sqrt() } diff --git a/src/solver.rs b/src/solver.rs index 19dea88..587ea56 100644 --- a/src/solver.rs +++ b/src/solver.rs @@ -1,12 +1,14 @@ //! PVT solver use std::collections::HashMap; -use hifitime::Epoch; +use hifitime::{Epoch, Unit}; use log::{debug, error, warn}; +use map_3d::deg2rad; use thiserror::Error; use nyx::cosmic::eclipse::{eclipse_state, EclipseState}; use nyx::cosmic::Orbit; +use nyx::cosmic::SPEED_OF_LIGHT; use nyx::md::prelude::{Arc, Cosm}; use nyx::md::prelude::{Bodies, Frame, LightTimeCalc}; @@ -89,6 +91,8 @@ pub enum Error { PhysicalNonSenseRxPriorTx, #[error("physical non sense: t_rx is too late")] PhysicalNonSenseRxTooLate, + #[error("solution invalidated: gdop too large {0:.3E}")] + GDOPInvalidation(f64), } /// Interpolation result (state vector) that needs to be @@ -116,7 +120,16 @@ impl InterpolationResult { pub(crate) fn orbit(&self, dt: Epoch, frame: Frame) -> Orbit { let (x, y, z) = self.position(); let (v_x, v_y, v_z) = self.velocity().unwrap_or((0.0_f64, 0.0_f64, 0.0_f64)); - Orbit::cartesian(x, y, z, v_x / 1000.0, v_y / 1000.0, v_z / 1000.0, dt, frame) + Orbit::cartesian( + x / 1000.0, + y / 1000.0, + z / 1000.0, + v_x / 1000.0, + v_y / 1000.0, + v_z / 1000.0, + dt, + frame, + ) } } @@ -147,6 +160,8 @@ where prev_pvt: Option<(Epoch, PVTSolution)>, /* prev. estimate for recursive impl. */ prev_state: Option, + /* prev. state vector for internal velocity determination */ + prev_sv_state: HashMap)>, } impl Option> Solver { @@ -163,13 +178,12 @@ impl Option> Solver /* * print some infos on latched config */ - if cfg.modeling.relativistic_clock_corr { - warn!("relativistic clock corr. is not feasible at the moment"); - } - if mode.is_spp() && cfg.min_sv_sunlight_rate.is_some() { warn!("eclipse filter is not meaningful when using spp strategy"); } + if cfg.modeling.relativistic_path_range { + warn!("relativistic path range cannot be modeled at the moment"); + } Ok(Self { mode, @@ -179,8 +193,9 @@ impl Option> Solver apriori, interpolator, cfg: cfg.clone(), - prev_pvt: Option::<(Epoch, PVTSolution)>::None, + prev_sv_state: HashMap::new(), prev_state: Option::::None, + prev_pvt: Option::<(Epoch, PVTSolution)>::None, }) } /// Try to resolve a PVTSolution at desired "t". @@ -215,30 +230,70 @@ impl Option> Solver self.apriori.geodetic.z, ); + let mode = self.mode; let modeling = self.cfg.modeling; + let solver_opts = &self.cfg.solver; let interp_order = self.cfg.interp_order; /* interpolate positions */ let mut pool: Vec = pool .iter() - .filter_map(|c| { + .filter_map(|mut c| { let (t_tx, dt_ttx) = c.transmission_time(&self.cfg).ok()?; if let Some(mut interpolated) = (self.interpolator)(t_tx, c.sv, interp_order) { let mut c = c.clone(); - if modeling.earth_rotation { - const EARTH_OMEGA_E_WGS84: f64 = 7.2921151467E-5; - - let we = EARTH_OMEGA_E_WGS84 * dt_ttx; - let (we_cos, we_sin) = (we.cos(), we.sin()); - - let r = Matrix3::::new( - we_cos, we_sin, 0.0_f64, -we_sin, we_cos, 0.0_f64, 0.0_f64, 0.0_f64, + let rot = match modeling.earth_rotation { + true => { + const EARTH_OMEGA_E_WGS84: f64 = 7.2921151467E-5; + let we = EARTH_OMEGA_E_WGS84 * dt_ttx; + let (we_cos, we_sin) = (we.cos(), we.sin()); + Matrix3::::new( + we_cos, we_sin, 0.0_f64, -we_sin, we_cos, 0.0_f64, 0.0_f64, + 0.0_f64, 1.0_f64, + ) + }, + false => Matrix3::::new( + 1.0_f64, 0.0_f64, 0.0_f64, 0.0_f64, 1.0_f64, 0.0_f64, 0.0_f64, 0.0_f64, 1.0_f64, - ); - - interpolated.position = r * interpolated.position; + ), + }; + + interpolated.position = rot * interpolated.position; + + if modeling.relativistic_clock_bias { + if interpolated.velocity.is_none() { + /* + * we're interested in determining inst. speed vector + * but the user did not provide it, let's eval. it ourselves + */ + if let Some((p_ttx, p_position)) = self.prev_sv_state.get(&c.sv) { + let dt = (t_tx - *p_ttx).to_seconds(); + interpolated.velocity = + Some((rot * interpolated.position - rot * p_position) / dt); + } + } + + if interpolated.velocity.is_some() { + // otherwise, following calaculations would diverge + let orbit = interpolated.orbit(t_tx, self.earth_frame); + const EARTH_SEMI_MAJOR_AXIS_WGS84: f64 = 6378137.0_f64; + const EARTH_GRAVITATIONAL_CONST: f64 = 3986004.418 * 10.0E8; + let orbit = interpolated.orbit(t_tx, self.earth_frame); + let ea_rad = deg2rad(orbit.ea_deg()); + let gm = + (EARTH_SEMI_MAJOR_AXIS_WGS84 * EARTH_GRAVITATIONAL_CONST).sqrt(); + let bias = -2.0_f64 * orbit.ecc() * ea_rad.sin() * gm + / SPEED_OF_LIGHT + / SPEED_OF_LIGHT + * Unit::Second; + debug!("{:?} ({}) : relativistic clock bias: {}", t_tx, c.sv, bias); + c.clock_corr += bias; + } + + self.prev_sv_state + .insert(c.sv, (t_tx, interpolated.position)); } debug!( @@ -277,17 +332,42 @@ impl Option> Solver /* remove observed signals above snr mask (if any) */ if let Some(min_snr) = self.cfg.min_snr { + let mut nb_removed: usize = 0; for idx in 0..pool.len() { - let (init_code, init_phase) = (pool[idx].code.len(), pool[idx].phase.len()); - pool[idx].min_snr_mask(min_snr); - let delta_code = init_code - pool[idx].code.len(); - let delta_phase = init_phase - pool[idx].phase.len(); + let (init_code, init_phase) = ( + pool[idx - nb_removed].code.len(), + pool[idx - nb_removed].phase.len(), + ); + pool[idx - nb_removed].min_snr_mask(min_snr); + let delta_code = init_code - pool[idx - nb_removed].code.len(); + let delta_phase = init_phase - pool[idx - nb_removed].phase.len(); if delta_code > 0 || delta_phase > 0 { debug!( "{:?} ({}) : {} code | {} phase below snr mask", - pool[idx].t, pool[idx].sv, delta_code, delta_phase + pool[idx - nb_removed].t, + pool[idx - nb_removed].sv, + delta_code, + delta_phase ); } + /* make sure we're still compliant with the strategy */ + match mode { + Mode::SPP | Mode::LSQSPP => { + if pool[idx - nb_removed].code.len() == 0 { + debug!("{:?} ({}) dropped on bad snr", pool[idx].t, pool[idx].sv); + let _ = pool.swap_remove(idx - nb_removed); + nb_removed += 1; + } + }, + Mode::PPP => { + let mut drop = !pool[idx - nb_removed].dual_freq_pseudorange(); + drop |= !pool[idx - nb_removed].dual_freq_phase(); + if drop { + let _ = pool.swap_remove(idx - nb_removed); + nb_removed += 1; + } + }, + } } } @@ -304,7 +384,7 @@ impl Option> Solver }; if eclipsed { debug!( - "{:?} ({}): earth eclipsed, dropping", + "{:?} ({}): dropped - eclipsed by earth", pool[idx].t, pool[idx].sv ); let _ = pool.swap_remove(idx); @@ -329,7 +409,7 @@ impl Option> Solver if let Ok(sv_data) = cd.resolve( t, &self.cfg, - self.mode, + mode, (x0, y0, z0), (lat_ddeg, lon_ddeg, altitude_above_sea_m), iono_bias, @@ -342,7 +422,7 @@ impl Option> Solver } } - let w = self.cfg.lsq_weight_matrix( + let w = self.cfg.solver.lsq_weight_matrix( nb_candidates, pvt_sv_data .iter() @@ -350,12 +430,40 @@ impl Option> Solver .collect(), ); - let mut pvt_solution = - PVTSolution::new(g.clone(), w, y, pvt_sv_data, self.prev_state.clone())?; + let mut pvt_solution = PVTSolution::new( + g.clone(), + w.clone(), + y.clone(), + pvt_sv_data, + self.prev_state.clone(), + )?; + + /* + * Solution validation : GDOP + */ + if let Some(max_gdop) = solver_opts.gdop_threshold { + let gdop = pvt_solution.gdop(); + if gdop > max_gdop { + // invalidate + return Err(Error::GDOPInvalidation(gdop)); + } + } + + if mode.is_recursive() { + /* + * Solution validation : residual vector + */ + let mut residuals = DVector::::zeros(nb_candidates); + for i in 0..nb_candidates { + residuals[i] = y[i] - 0.0_f64 / w[(i, i)]; + } + // let denom = m - n - 1; + // residuals.clone().transpose() * residuals / denom; + } if let Some((prev_t, prev_pvt)) = &self.prev_pvt { pvt_solution.v = (pvt_solution.p - prev_pvt.p) / (t - *prev_t).to_seconds(); - if self.mode.is_recursive() { + if mode.is_recursive() { self.prev_state = Some(pvt_solution.estimate.clone()); } } @@ -367,16 +475,16 @@ impl Option> Solver if let Some(alt) = self.cfg.fixed_altitude { pvt_solution.p.z = self.apriori.ecef.z - alt; pvt_solution.v.z = 0.0_f64; - pvt_solution.estimate.covar[(2, 2)] = 0.0_f64; + // pvt_solution.estimate.covar[(2, 2)] = 0.0_f64; } match solution { PVTSolutionType::TimeOnly => { pvt_solution.p = Vector3::::default(); pvt_solution.v = Vector3::::default(); - pvt_solution.estimate.covar[(0, 0)] = 0.0_f64; - pvt_solution.estimate.covar[(1, 1)] = 0.0_f64; - pvt_solution.estimate.covar[(2, 2)] = 0.0_f64; + // pvt_solution.estimate.covar[(0, 0)] = 0.0_f64; + // pvt_solution.estimate.covar[(1, 1)] = 0.0_f64; + // pvt_solution.estimate.covar[(2, 2)] = 0.0_f64; }, _ => {}, }