Develop

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "gnss-rtk"
-version = "0.3.1"
+version = "0.4.0"
 license = "MIT OR Apache-2.0"
 authors = ["Guillaume W. Bres <[email protected]>"]
 description = "GNSS position solver"

src/candidate.rs

@@ -4,7 +4,6 @@ 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};
@@ -241,7 +240,8 @@ impl Candidate {
         let (x0, y0, z0) = apriori;
         let clock_corr = self.clock_corr.to_seconds();
         let (azimuth, elevation) = (state.azimuth, state.elevation);
-        let (sv_x, sv_y, sv_z) = state.position();
+        let sv_p = state.position();
+        let (sv_x, sv_y, sv_z) = (sv_p[0], sv_p[1], sv_p[2]);
 
         let mut sv_data = PVTSVData::default();
         sv_data.azimuth = azimuth;

src/cfg.rs

@@ -4,7 +4,7 @@ use thiserror::Error;
 use serde::{Deserialize, Serialize};
 
 use crate::prelude::{Mode, TimeScale};
-use nalgebra::{DMatrix, DVector, Matrix3, MatrixXx4, Vector3};
+use nalgebra::DMatrix;
 
 /// Configuration Error
 #[derive(Debug, Error)]
@@ -83,7 +83,26 @@ fn default_relativistic_path_range() -> bool {
     false
 }
 
+fn default_sv_apc() -> bool {
+    false
+}
+
 fn default_lsq_weight() -> Option<LSQWeight> {
+    //Some(LSQWeight::LSQWeightMappingFunction(
+    //    ElevationMappingFunction {
+    //        a: 5.0,
+    //        b: 0.0,
+    //        c: 10.0,
+    //    },
+    //))
+    None
+}
+
+fn default_gdop_threshold() -> Option<f64> {
+    Some(1.0)
+}
+
+fn default_innov_threshold() -> Option<f64> {
     None
 }
 
@@ -107,10 +126,14 @@ pub struct InternalDelay {
 #[cfg_attr(feature = "serde", derive(Deserialize))]
 pub struct SolverOpts {
     /// GDOP Threshold (max. limit) to invalidate ongoing GDOP
+    #[cfg_attr(feature = "serde", serde(default = "default_gdop_threshold"))]
     pub gdop_threshold: Option<f64>,
     /// Weight Matrix in LSQ solving process
     #[cfg_attr(feature = "serde", serde(default = "default_lsq_weight"))]
     pub lsq_weight: Option<LSQWeight>,
+    /// Threshold on new filter innovation
+    #[cfg_attr(feature = "serde", serde(default = "default_innov_threshold"))]
+    pub innovation_threshold: Option<f64>,
 }
 
 impl SolverOpts {
@@ -123,7 +146,8 @@ impl SolverOpts {
             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();
+                    let sigma = mapf.a + mapf.b * ((-sv_elev[i]) / mapf.c).exp();
+                    mat[(i, i)] = 1.0 / sigma.powi(2);
                 }
             },
             None => {},
@@ -141,6 +165,8 @@ pub struct Modeling {
     #[cfg_attr(feature = "serde", serde(default))]
     pub sv_total_group_delay: bool,
     #[cfg_attr(feature = "serde", serde(default))]
+    pub sv_apc: bool,
+    #[cfg_attr(feature = "serde", serde(default))]
     pub relativistic_clock_bias: bool,
     #[cfg_attr(feature = "serde", serde(default))]
     pub relativistic_path_range: bool,
@@ -156,6 +182,7 @@ impl Default for Modeling {
     fn default() -> Self {
         Self {
             sv_clock_bias: default_sv_clock(),
+            sv_apc: default_sv_apc(),
             iono_delay: default_iono(),
             tropo_delay: default_tropo(),
             sv_total_group_delay: default_sv_tgd(),
@@ -247,8 +274,9 @@ impl Config {
                 int_delay: Default::default(),
                 externalref_delay: Default::default(),
                 solver: SolverOpts {
+                    gdop_threshold: default_gdop_threshold(),
                     lsq_weight: None,
-                    gdop_threshold: None,
+                    innovation_threshold: None,
                 },
             },
             Mode::LSQSPP => Self {
@@ -257,21 +285,16 @@ impl Config {
                 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(),
                 solver: SolverOpts {
-                    gdop_threshold: Some(30.0),
-                    lsq_weight: Some(LSQWeight::LSQWeightMappingFunction(
-                        ElevationMappingFunction {
-                            a: 0.03,
-                            b: 0.03,
-                            c: 100.0,
-                        },
-                    )),
+                    gdop_threshold: default_gdop_threshold(),
+                    lsq_weight: default_lsq_weight(),
+                    innovation_threshold: default_innov_threshold(),
                 },
             },
             Mode::PPP => Self {
@@ -288,14 +311,9 @@ impl Config {
                 int_delay: Default::default(),
                 externalref_delay: Default::default(),
                 solver: SolverOpts {
-                    gdop_threshold: None, //TODO
-                    lsq_weight: Some(LSQWeight::LSQWeightMappingFunction(
-                        ElevationMappingFunction {
-                            a: 0.03,
-                            b: 0.03,
-                            c: 100.0,
-                        },
-                    )),
+                    gdop_threshold: default_gdop_threshold(),
+                    lsq_weight: default_lsq_weight(),
+                    innovation_threshold: default_innov_threshold(),
                 },
             },
         }
@@ -304,10 +322,10 @@ impl Config {
 
 #[derive(Default, Debug, Clone, Copy, PartialEq)]
 #[cfg_attr(feature = "serde", derive(Deserialize))]
-pub enum SolverMode {
-    /// Receiver is kept at fixed location
+pub enum PositioningMode {
+    /// Receiver is static
     #[default]
     Static,
-    /// Receiver is not static
+    /// Receiver is moving
     Kinematic,
 }

src/lib.rs

@@ -22,7 +22,7 @@ pub mod prelude {
     pub use crate::candidate::{Candidate, Observation};
     pub use crate::cfg::Config;
     pub use crate::solutions::{PVTSolution, PVTSolutionType};
-    pub use crate::solver::{InterpolationResult, Mode, Solver};
+    pub use crate::solver::{InterpolatedPosition, InterpolationResult, Mode, Solver};
     // re-export
     pub use gnss::prelude::{Constellation, SV};
     pub use hifitime::{Duration, Epoch, TimeScale};

src/solutions.rs → src/solutions/mod.rs

@@ -3,6 +3,8 @@ use crate::prelude::{Vector3, SV};
 use crate::Error;
 use std::collections::HashMap;
 
+pub(crate) mod validator;
+
 #[derive(Debug, Copy, Clone, Default)]
 pub enum PVTSolutionType {
     /// Default, complete solution with Position,
@@ -27,7 +29,7 @@ impl std::fmt::Display for PVTSolutionType {
     }
 }
 
-use nalgebra::base::{DMatrix, DVector, Matrix1x4, Matrix4, Matrix4x1, MatrixXx4};
+use nalgebra::base::{DMatrix, DVector, Matrix3, Matrix4, Matrix4x1, MatrixXx4};
 use nyx::cosmic::SPEED_OF_LIGHT;
 
 #[cfg(feature = "serde")]
@@ -87,10 +89,10 @@ pub struct PVTSVData {
 
 #[derive(Default, Clone, Debug)]
 pub struct Estimate {
-    /* x: estimate */
+    /* x estimate */
     pub(crate) x: Matrix4x1<f64>,
-    /* and its covariance matrix */
-    pub(crate) covar: Matrix4<f64>,
+    /* Q matrix */
+    pub(crate) q: Matrix4<f64>,
 }
 
 /// PVT Solution, always expressed as the correction to apply
@@ -132,21 +134,21 @@ impl PVTSolution {
                     .try_inverse()
                     .ok_or(Error::MatrixInversionError)?;
                 let x = (q * g_prime.clone()) * w.clone() * y;
-                Estimate { x, covar: q }
+                Estimate { x, q }
             },
             Some(state) => {
                 let p_1 = state
-                    .covar
+                    .q
                     .try_inverse()
                     .ok_or(Error::CovarMatrixInversionError)?;
 
                 let q = g_prime.clone() * w.clone() * g.clone();
-                let covar = (p_1 + q)
+                let q = (p_1 + q)
                     .try_inverse()
                     .ok_or(Error::CovarMatrixInversionError)?;
 
-                let x = covar * (p_1 * state.x + (g_prime.clone() * w.clone() * y));
-                Estimate { x, covar }
+                let x = q * (p_1 * state.x + (g_prime.clone() * w.clone() * y));
+                Estimate { x, q }
             },
         };
 
@@ -167,24 +169,55 @@ impl PVTSolution {
     pub fn sv(&self) -> Vec<SV> {
         self.sv.keys().copied().collect()
     }
-    /// Returns Geometrical Dilution of Precision of Self
+    /// Returns Geometric 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)])
+        (self.estimate.q[(0, 0)]
+            + self.estimate.q[(1, 1)]
+            + self.estimate.q[(2, 2)]
+            + self.estimate.q[(3, 3)])
             .sqrt()
     }
+    /// Returns Position Diultion of Precision of Self
+    pub fn pdop(&self) -> f64 {
+        (self.estimate.q[(0, 0)] + self.estimate.q[(1, 1)] + self.estimate.q[(2, 2)]).sqrt()
+    }
+    fn q_enu(&self, lat: f64, lon: f64) -> Matrix3<f64> {
+        let r = Matrix3::<f64>::new(
+            -lon.sin(),
+            -lon.cos() * lat.sin(),
+            lat.cos() * lon.cos(),
+            lon.cos(),
+            -lat.sin() * lon.sin(),
+            lat.cos() * lon.sin(),
+            0.0_f64,
+            lat.cos(),
+            lon.sin(),
+        );
+        let q_3 = Matrix3::<f64>::new(
+            self.estimate.q[(0, 0)],
+            self.estimate.q[(0, 1)],
+            self.estimate.q[(0, 2)],
+            self.estimate.q[(1, 0)],
+            self.estimate.q[(1, 1)],
+            self.estimate.q[(1, 2)],
+            self.estimate.q[(2, 0)],
+            self.estimate.q[(2, 1)],
+            self.estimate.q[(2, 2)],
+        );
+        r.clone().transpose() * q_3 * r.clone()
+    }
     /// Returns Horizontal Dilution of Precision of Self
-    pub fn hdop(&self) -> f64 {
-        (self.estimate.covar[(0, 0)] + self.estimate.covar[(1, 1)]).sqrt()
+    pub fn hdop(&self, lat: f64, lon: f64) -> f64 {
+        let q_enu = self.q_enu(lat, lon);
+        (q_enu[(0, 0)] + q_enu[(1, 1)]).sqrt()
     }
     /// Returns Vertical Dilution of Precision of Self
-    pub fn vdop(&self) -> f64 {
-        self.estimate.covar[(2, 2)].sqrt()
+    pub fn vdop(&self, lat: f64, lon: f64) -> f64 {
+        let q_enu = self.q_enu(lat, lon);
+        q_enu[(1, 1)].sqrt()
     }
     /// Returns Time Dilution of Precision of Self
     pub fn tdop(&self) -> f64 {
-        self.estimate.covar[(3, 3)].sqrt()
+        self.estimate.q[(3, 3)].sqrt()
     }
 }

src/solutions/validator.rs

@@ -0,0 +1,90 @@
+use log::debug;
+use nalgebra::{DMatrix, DVector, Vector3};
+use nyx::cosmic::SPEED_OF_LIGHT;
+use thiserror::Error;
+
+use crate::{
+    cfg::SolverOpts,
+    prelude::{Candidate, PVTSolution},
+};
+
+#[derive(Clone, Debug, Error)]
+pub enum SolutionInvalidation {
+    #[error("gdop limit exceeded {0}")]
+    GDOPOutlier(f64),
+    #[error("innovation outlier |{0}|")]
+    InnovationOutlier(f64),
+    #[error("coderes limit exceeded {0}")]
+    CodeResidual(f64),
+}
+
+pub(crate) struct SolutionValidator {
+    gdop: f64,
+    residuals: DVector<f64>,
+}
+
+impl SolutionValidator {
+    pub fn new(
+        apriori_ecef: &Vector3<f64>,
+        pool: &Vec<Candidate>,
+        w: &DMatrix<f64>,
+        solution: &PVTSolution,
+    ) -> Self {
+        let gdop = solution.gdop();
+        let mut residuals = DVector::<f64>::zeros(pool.len());
+
+        for (idx, cd) in pool.iter().enumerate() {
+            let sv = solution
+                .sv
+                .iter()
+                .filter_map(|(sv, data)| if *sv == cd.sv { Some(data) } else { None })
+                .reduce(|k, _| k)
+                .unwrap();
+
+            let pr = cd.prefered_pseudorange().unwrap().value;
+            let state = cd.state.unwrap().position();
+            let estimate = apriori_ecef + solution.p;
+
+            let (sv_x, sv_y, sv_z) = (state[0], state[1], state[2]);
+            let (x, y, z) = (estimate[0], estimate[1], estimate[2]);
+
+            let rho = ((sv_x - x).powi(2) + (sv_y - y).powi(2) + (sv_z - z).powi(2)).sqrt();
+
+            let dt = cd.clock_corr.to_seconds() - solution.dt;
+
+            residuals[idx] = pr;
+            residuals[idx] -= rho;
+            residuals[idx] += dt * SPEED_OF_LIGHT;
+            residuals[idx] -= sv.tropo_bias.value().unwrap_or(0.0);
+            residuals[idx] -= sv.iono_bias.value().unwrap_or(0.0);
+            residuals[idx] /= w[(idx, idx)];
+            debug!(
+                "{:?} ({}): coderes={}/w={}",
+                cd.t,
+                cd.sv,
+                residuals[idx],
+                w[(idx, idx)]
+            );
+        }
+        Self { residuals, gdop }
+    }
+    /*
+     * Solution validation process
+     */
+    pub fn valid(&self, opts: &SolverOpts) -> Result<(), SolutionInvalidation> {
+        if let Some(max_gdop) = opts.gdop_threshold {
+            if self.gdop > max_gdop {
+                return Err(SolutionInvalidation::GDOPOutlier(self.gdop));
+            }
+        }
+        if let Some(threshold) = opts.innovation_threshold {
+            for idx in 0..self.residuals.len() {
+                let res = self.residuals[idx].abs();
+                if res > threshold {
+                    return Err(SolutionInvalidation::InnovationOutlier(res));
+                }
+            }
+        }
+        Ok(())
+    }
+}