Introducing GNSS position solver (#169)

---------

Signed-off-by: Guillaume W. Bres <[email protected]>
Co-authored-by: Laurențiu Nicola <[email protected]>

Cargo.toml

@@ -1,13 +1,14 @@
 [workspace]
 resolver = "2"
 members = [
-    "rinex", 
+    "gnss-rtk",
     "crx2rnx", 
-    "rnx2crx", 
     "qc-traits", 
+    "rinex", 
     "rinex-qc", 
     "rinex-cli", 
-    "ublox-rnx", 
+    "rnx2crx", 
     "sinex",
     "sp3",
+    "ublox-rnx", 
 ]

README.md

@@ -31,10 +31,11 @@ at least run a -qc analysis, and possibly the position solver once it is merged
 - Supports high precision RINEX (scaled phase data with micro cycle precision)
 - RINEX post processing like SNR, DCB analysis, Broadcast ephemeris interpolation,
 high precision orbit interpolation (SP3)..
-- RINEX-qc : statistical analysis like "teqc", including on modern signals and SP3 high precision orbits
-- An SPP/PPP position solver is under develoment:
-checkout [this branch](https://github.com/georust/rinex/tree/solver) which
-is kept up to date until merged
+- RINEX-qc: statistical analysis that you can request in the "cli" application directly.
+Analysis can run on modern GNSS signals and SP3 high precision data.
+Emulates "teqc" historical application.
+- An SPP/PPP position solver (under development), in the form of the "gnss-rtk" library that you can
+summon from the "cli" application directly.
 
 ## Known weaknesses :warning:
 
@@ -54,6 +55,8 @@ The application is auto-generated for a few architectures, download it from the
 [release portal](https://github.com/gwbres/rinex/releases)
 
 * [`sp3`](sp3/) High Precision Orbits (by IGS) 
+* [`gnss-rtk`](gnss-rtk/) a position solver from raw GNSS signals.
+Currently works from RINEX input data, but that is not exclusive.
 * [`rnx2crx`](rnx2crx/) is a RINEX compressor (RINEX to Compact RINEX)
 * [`crx2rnx`](crx2rnx/) is a CRINEX decompresor (Compact RINEX to RINEX)
 * [`rinex-qc`](rinex-qc/) is a library dedicated to RINEX files analysis 

gnss-rtk/Cargo.toml

@@ -0,0 +1,19 @@
+[package]
+name = "gnss-rtk"
+version = "0.0.1"
+license = "MIT OR Apache-2.0"
+authors = ["Guillaume W. Bres <[email protected]>"]
+description = "GNSS position solver"
+homepage = "https://github.com/georust/rinex/gnss-rtk"
+repository = "https://github.com/georust/rinex/gnss-rtk"
+keywords = ["timing", "positioning", "gps", "glonass", "galileo"]
+categories = ["science", "science::geo"]
+edition = "2021"
+readme = "README.md"
+
+[dependencies]
+log = "0.4"
+thiserror = "1"
+nyx-space = "2.0.0-alpha.2" 
+rinex-qc = { path = "../rinex-qc", features = ["serde"] }
+rinex = { path = "../rinex", features = ["serde", "flate2", "sbas", "obs", "nav", "qc", "processing"] }

gnss-rtk/README.md

@@ -0,0 +1,8 @@
+GNSS-RTK
+========
+
+Precise position solver.
+
+The Solver can work from a RINEX context blob, defined in this crate toolsuite, but is not exclusively tied to RINEX.
+
+The solver implements precise positioning algorithms, which are based on raw GNSS signals.

gnss-rtk/src/lib.rs

@@ -0,0 +1,302 @@
+use nyx_space::cosmic::eclipse::{eclipse_state, EclipseState};
+use nyx_space::cosmic::{Orbit, SPEED_OF_LIGHT};
+use nyx_space::md::prelude::{Bodies, LightTimeCalc};
+use rinex::prelude::{Duration, Epoch, Sv};
+use rinex_qc::QcContext;
+use std::collections::HashMap;
+
+extern crate nyx_space as nyx;
+
+use nyx::md::prelude::{Arc, Cosm};
+
+mod models;
+mod opts;
+
+pub mod prelude {
+    pub use crate::opts::PositioningMode;
+    pub use crate::opts::SolverOpts;
+    pub use crate::Solver;
+    pub use crate::SolverEstimate;
+    pub use crate::SolverType;
+}
+
+use opts::SolverOpts;
+
+use log::{debug, trace, warn};
+
+use thiserror::Error;
+
+#[derive(Debug, Clone, Copy, Error)]
+pub enum Error {
+    #[error("provided context is either unsufficient or invalid for any position solving")]
+    Unfeasible,
+}
+
+#[derive(Default, Debug, Clone, Copy, PartialEq)]
+pub enum SolverType {
+    /// SPP : code based and approximated models
+    /// aiming a metric resolution.
+    #[default]
+    SPP,
+    /// PPP : phase + code based, the ultimate solver
+    /// aiming a millimetric resolution.
+    PPP,
+}
+
+impl std::fmt::Display for SolverType {
+    fn fmt(&self, fmt: &mut std::fmt::Formatter) -> std::fmt::Result {
+        match self {
+            Self::SPP => write!(fmt, "SPP"),
+            Self::PPP => write!(fmt, "PPP"),
+        }
+    }
+}
+
+impl SolverType {
+    fn from(ctx: &QcContext) -> Result<Self, Error> {
+        if ctx.primary_data().is_observation_rinex() {
+            if ctx.has_sp3() {
+                Ok(Self::PPP)
+            } else {
+                if ctx.has_navigation_data() {
+                    Ok(Self::SPP)
+                } else {
+                    Err(Error::Unfeasible)
+                }
+            }
+        } else {
+            Err(Error::Unfeasible)
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct Solver {
+    /// Cosmic model
+    cosmic: Arc<Cosm>,
+    /// Solver parametrization
+    pub opts: SolverOpts,
+    /// Whether this solver is initiated (ready to iterate) or not
+    initiated: bool,
+    /// Type of solver implemented
+    pub solver: SolverType,
+    /// Current epoch
+    nth_epoch: usize,
+    /// current estimate
+    pub estimate: SolverEstimate,
+}
+
+#[derive(Debug, Copy, Clone, Default)]
+pub struct SolverEstimate {
+    /// Position estimate
+    pub pos: (f64, f64, f64),
+    /// Time offset estimate
+    pub clock_offset: Duration,
+}
+
+impl Solver {
+    pub fn from(context: &QcContext) -> Result<Self, Error> {
+        let solver = SolverType::from(context)?;
+        Ok(Self {
+            cosmic: Cosm::de438(),
+            solver,
+            initiated: false,
+            opts: SolverOpts::default(solver),
+            nth_epoch: 0,
+            estimate: SolverEstimate::default(),
+        })
+    }
+    pub fn init(&mut self, ctx: &mut QcContext) {
+        trace!("{} solver initialization..", self.solver);
+        //TODO: Preprocessing:
+        //      only for ppp solver
+        //      preseve "complete" epochs only
+        //        incomplete epochs will not be considered by PPP solver
+        //        this reduces nb of Epochs to interpolate
+        // trace!("\"complete\" epoch filter..");
+
+        // let total = ctx.primary_data().epoch().count();
+        // ctx.complete_epoch_filter_mut(None);
+        // let total_dropped = total - ctx.primary_data().epoch().count();
+        // trace!(
+        //     "dropped a total of {}/{} \"incomplete\" epochs",
+        //     total_dropped,
+        //     total
+        // );
+
+        // 2: interpolate: if need be
+        //if !ctx.interpolated {
+        //    trace!("orbit interpolation..");
+        //    let order = self.opts.interp_order;
+        //    ctx.orbit_interpolation(order, None);
+        //    //TODO could be nice to have some kind of timing/perf evaluation here
+        //    //     and also total number of required interpolations
+        //}
+
+        //initialization
+        self.nth_epoch = 0;
+        self.estimate.pos = self.opts.rcvr_position.into();
+        self.initiated = true;
+    }
+    pub fn run(&mut self, ctx: &mut QcContext) -> Option<(Epoch, SolverEstimate)> {
+        if !self.initiated {
+            self.init(ctx);
+            trace!("solver initiated");
+        } else {
+            // move on to next epoch
+            self.nth_epoch += 1;
+        }
+
+        // grab work instant
+        let t = ctx.primary_data().epoch().nth(self.nth_epoch)?;
+
+        let interp_order = self.opts.interp_order;
+
+        /* elect vehicles */
+        let elected_sv = Self::sv_election(ctx, t);
+        if elected_sv.is_none() {
+            warn!("no vehicles elected @ {}", t);
+            return Some((t, self.estimate));
+        }
+
+        let mut elected_sv = elected_sv.unwrap();
+        debug!("elected sv : {:?}", elected_sv);
+
+        /* determine sv positions */
+        /* TODO: SP3 APC corrections: Self::eval_sun_vector3d */
+
+        let mut sv_pos: HashMap<Sv, (f64, f64, f64)> = HashMap::new();
+        for sv in &elected_sv {
+            if let Some(sp3) = ctx.sp3_data() {
+                if let Some((x_km, y_km, z_km)) = sp3.sv_position_interpolate(*sv, t, interp_order)
+                {
+                    sv_pos.insert(*sv, (x_km, y_km, z_km));
+                } else if let Some(nav) = ctx.navigation_data() {
+                    if let Some((x_km, y_km, z_km)) =
+                        nav.sv_position_interpolate(*sv, t, interp_order)
+                    {
+                        sv_pos.insert(*sv, (x_km, y_km, z_km));
+                    }
+                }
+            } else {
+                if let Some(nav) = ctx.navigation_data() {
+                    if let Some((x_km, y_km, z_km)) =
+                        nav.sv_position_interpolate(*sv, t, interp_order)
+                    {
+                        sv_pos.insert(*sv, (x_km, y_km, z_km));
+                    }
+                }
+            }
+        }
+
+        /* remove sv in eclipse */
+        if let Some(min_rate) = self.opts.min_sv_sunlight_rate {
+            sv_pos.retain(|sv, (x_km, y_km, z_km)| {
+                let state = self.eclipse_state(*x_km, *y_km, *z_km, t);
+                let eclipsed = match state {
+                    EclipseState::Umbra => true,
+                    EclipseState::Visibilis => false,
+                    EclipseState::Penumbra(r) => {
+                        debug!("{} state: {}", sv, state);
+                        r < min_rate
+                    },
+                };
+
+                if eclipsed {
+                    debug!("dropping eclipsed {}", sv);
+                }
+                !eclipsed
+            });
+        }
+
+        // 3: t_tx
+        let mut t_tx: HashMap<Sv, Epoch> = HashMap::new();
+        for sv in &elected_sv {
+            if let Some(sv_t_tx) = Self::sv_transmission_time(ctx, *sv, t) {
+                t_tx.insert(*sv, sv_t_tx);
+            }
+        }
+
+        //TODO
+        // add other models
+
+        // form matrix
+        // resolve
+        Some((t, self.estimate))
+    }
+    /*
+     * Evalutes T_tx transmission time, for given Sv at desired 't'
+     */
+    fn sv_transmission_time(ctx: &QcContext, sv: Sv, t: Epoch) -> Option<Epoch> {
+        let nav = ctx.navigation_data()?;
+        // need one pseudo range observation for this SV @ 't'
+        let mut pr = ctx
+            .primary_data()
+            .pseudo_range()
+            .filter_map(|((e, flag), svnn, _, p)| {
+                if e == t && flag.is_ok() && svnn == sv {
+                    Some(p)
+                } else {
+                    None
+                }
+            })
+            .take(1);
+        if let Some(pr) = pr.next() {
+            let t_tx = Duration::from_seconds(t.to_duration().to_seconds() - pr / SPEED_OF_LIGHT);
+            debug!("t_tx(pr): {}@{} : {}", sv, t, t_tx);
+
+            let mut e_tx = Epoch::from_duration(t_tx, sv.constellation.timescale()?);
+            let dt_sat = nav.sv_clock_bias(sv, e_tx)?;
+            debug!("clock bias: {}@{} : {}", sv, t, dt_sat);
+
+            e_tx -= dt_sat;
+            debug!("{} : t(obs): {} | t(tx) {}", sv, t, e_tx);
+
+            Some(e_tx)
+        } else {
+            debug!("missing PR measurement");
+            None
+        }
+    }
+    /*
+     * Evaluates Sun/Earth vector, <!> expressed in Km <!>
+     * for all SV NAV Epochs in provided context
+     */
+    #[allow(dead_code)]
+    fn eval_sun_vector3d(&mut self, ctx: &QcContext, t: Epoch) -> (f64, f64, f64) {
+        let sun_body = Bodies::Sun;
+        let eme_j2000 = self.cosmic.frame("EME2000");
+        let orbit =
+            self.cosmic
+                .celestial_state(sun_body.ephem_path(), t, eme_j2000, LightTimeCalc::None);
+        (orbit.x_km, orbit.y_km, orbit.z_km)
+    }
+    /*
+     * Computes celestial angle condition
+     */
+    fn eclipse_state(&self, x_km: f64, y_km: f64, z_km: f64, epoch: Epoch) -> EclipseState {
+        let sun_frame = self.cosmic.frame("Sun J2000");
+        let earth_frame = self.cosmic.frame("EME2000");
+        let sv_orbit = Orbit {
+            x_km,
+            y_km,
+            z_km,
+            vx_km_s: 0.0_f64,
+            vy_km_s: 0.0_f64,
+            vz_km_s: 0.0_f64,
+            epoch,
+            frame: earth_frame,
+            stm: None,
+        };
+        eclipse_state(&sv_orbit, sun_frame, earth_frame, &self.cosmic)
+    }
+    /*
+     * Elects sv for this epoch
+     */
+    fn sv_election(ctx: &QcContext, t: Epoch) -> Option<Vec<Sv>> {
+        ctx.primary_data()
+            .sv_epoch()
+            .filter_map(|(epoch, svs)| if epoch == t { Some(svs) } else { None })
+            .reduce(|svs, _| svs)
+    }
+}

gnss-rtk/src/models.rs

@@ -0,0 +1,15 @@
+#[derive(Default, Debug, Copy, Clone)]
+pub struct Models {
+    pub tropo: Option<TropoModel>,
+    pub iono: Option<IonoModel>,
+    pub tgd: Option<TGDModel>,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub struct TropoModel {}
+
+#[derive(Debug, Copy, Clone)]
+pub struct IonoModel {}
+
+#[derive(Debug, Copy, Clone)]
+pub struct TGDModel {}