Replace OrbitalState with ANISE::Orbit (#33)

* Replace OrbitalState with ANISE::Orbit
* reestablish group delay

---------

Signed-off-by: Guillaume W. Bres <guillaume.bressaix@gmail.com>

Author: gwbres

README.md

@@ -22,9 +22,22 @@ GNSS-RTK is flexible and efficient:
 * you don't have to sample L1 and can navigate with modern signals
 * it supports navigation without phase range
 * a special CPP method for dual frequency pseudo range (no phase range)
-which is pretty much a slow converging PPP method
-* it can operate without apriori knowledge and is a true surveying tool
+which behaves like a slow converging PPP method
+* is a true surveying tool because it can operate without apriori knowledge
 * it can fulfill the challenging task of RTK / Geodetic reference station calibration
+by deploying a complete PPP survey
+
+Other cool features:
+  * works in all supported timescales
+  * can navigate using a conic azimuth mask (min and max azimuth angle).
+  In this case, we only select vehicles from that very region of the compass.
+  * could potentially apply to other Planets, if we make some function more generic
+  and propose better atmosphere interfaces.
+
+GNSS-RTK does not care about SV or signal modulations. It cares
+about physics, distances, frequencies and environmental phenomena.
+This means you operate it from whatever data source you have at your disposal,
+as long as you can provide the required inputs.
 
 Applications
 ============

examples/spp/main.rs

@@ -1,25 +1,24 @@
 // SPP example (pseudo range based direct positioning).
 // This is simply here to demonstrate how to operate the API, and does not generate actual results.
 use gnss_rtk::prelude::{
-    Candidate, Carrier, ClockCorrection, Config, Duration, Epoch, Error, InvalidationCause,
-    IonoComponents, Method, Observation, OrbitalState, OrbitalStateProvider, Solver,
-    TropoComponents, SV,
+    Candidate, Carrier, Config, Epoch, Error, Frame, InvalidationCause, Method, Observation, Orbit,
+    OrbitSource, Solver, EARTH_J2000, SV,
 };
 
 // Orbit source example
 struct Orbits {}
 
-impl OrbitalStateProvider for Orbits {
+impl OrbitSource for Orbits {
     // For each requested "t" and "sv",
-    // if we can, we should resolve the SV [OrbitalState].
+    // if we can, we should resolve the SV [Orbit].
     // If interpolation is to be used (depending on your apps), you can
     // use the interpolation order that we recommend here, or decide to ignore it.
-    // If you're not in position to determine [OrbitalState], simply return None.
+    // If you're not in position to determine [Orbit], simply return None.
     // If None is returned for too long, this [Epoch] will eventually be dropped out,
     // and we will move on to the next
-    fn next_at(&mut self, t: Epoch, sv: SV, order: usize) -> Option<OrbitalState> {
-        let (x, y, z) = (0.0_f64, 0.0_f64, 0.0_f64);
-        Some(OrbitalState::from_position((x, y, z)))
+    fn next_at(&mut self, t: Epoch, _sv: SV, _fr: Frame, _order: usize) -> Option<Orbit> {
+        let (x_km, y_km, z_km) = (0.0_f64, 0.0_f64, 0.0_f64);
+        Some(Orbit::from_position(x_km, y_km, z_km, t, EARTH_J2000))
     }
 }
 
@@ -92,17 +91,10 @@ pub fn main() {
     while let Some((epoch, candidates)) = source.next() {
         match solver.resolve(epoch, &candidates) {
             Ok((_epoch, solution)) => {
-                // A solution was successfully resolved for this Epoch.
-                // The position is expressed as absolute ECEF [m].
-                let _position = solution.position;
-                // The velocity vector is expressed as variations of absolute ECEF positions [m/s]
-                let _velocity = solution.velocity;
                 // Receiver offset to preset timescale
                 let (_clock_offset, _timescale) = (solution.dt, solution.timescale);
                 // More infos on SVs that contributed to this solution
                 for info in solution.sv.values() {
-                    // attitude
-                    let (_el, _az) = (info.azimuth, info.elevation);
                     // Modeled (in this example) or simply copied ionosphere bias
                     // impacting selected signal from this spacecraft
                     let _bias_m = info.iono_bias;

src/bancroft.rs

@@ -1,5 +1,6 @@
 //! Brancroft solver
 use crate::{prelude::Candidate, solver::Error};
+use log::error;
 
 use nalgebra::{Matrix4, Vector4};
 use nyx_space::cosmic::SPEED_OF_LIGHT_M_S;
@@ -31,40 +32,59 @@ impl Bancroft {
         let m = Self::m_matrix();
         let mut a = Vector4::<f64>::default();
         let mut b = Matrix4::<f64>::default();
-        assert!(
-            cd.len() > 3,
-            "can't resolve Bancroft equation: invalid input"
-        );
-
-        for i in 0..4 {
-            let state = cd[i].state.ok_or(Error::UnresolvedState)?;
-            let (x_i, y_i, z_i) = (state.position[0], state.position[1], state.position[2]);
-            let r_i = cd[i]
-                .prefered_pseudorange()
-                .ok_or(Error::MissingPseudoRange)?
-                .pseudo
-                .unwrap();
+        if cd.len() < 4 {
+            return Err(Error::NotEnoughCandidatesBancroft);
+        }
 
-            let clock_corr = cd[i].clock_corr.ok_or(Error::UnknownClockCorrection)?;
-            let dt_i = clock_corr.duration.to_seconds();
-            let tgd_i = cd[i].tgd.unwrap_or_default().to_seconds();
+        let mut j = 0;
+        for i in 0..cd.len() {
+            if let Some(orbit) = cd[i].orbit {
+                let state = orbit.to_cartesian_pos_vel() * 1.0E3;
+                let (x_i, y_i, z_i) = (state[0], state[1], state[2]);
 
-            b[(i, 0)] = x_i;
-            b[(i, 1)] = y_i;
-            b[(i, 2)] = z_i;
-            let pr_i = r_i + dt_i * SPEED_OF_LIGHT_M_S - tgd_i;
-            b[(i, 3)] = pr_i;
-            a[i] = 0.5 * (x_i.powi(2) + y_i.powi(2) + z_i.powi(2) - pr_i.powi(2));
+                if let Some(r_i) = cd[i].prefered_pseudorange() {
+                    let r_i = r_i.pseudo.unwrap();
+                    if let Some(clock_corr) = cd[i].clock_corr {
+                        let dt_i = clock_corr.duration.to_seconds();
+                        let tgd_i = cd[i].tgd.unwrap_or_default().to_seconds();
+                        let pr_i = r_i + dt_i * SPEED_OF_LIGHT_M_S - tgd_i;
+                        b[(j, 0)] = x_i;
+                        b[(j, 1)] = y_i;
+                        b[(j, 2)] = z_i;
+                        b[(j, 3)] = pr_i;
+                        a[j] = 0.5 * (x_i.powi(2) + y_i.powi(2) + z_i.powi(2) - pr_i.powi(2));
+                        j += 1;
+                        if j == 4 {
+                            break;
+                        }
+                    } else {
+                        error!(
+                            "{}({}) bancroft needs onboard clock correction",
+                            cd[i].t, cd[i].sv
+                        );
+                    }
+                } else {
+                    error!("{}({}) bancroft needs 1 pseudo range", cd[i].t, cd[i].sv);
+                }
+            } else {
+                error!(
+                    "{}({}) bancroft unresolved orbital state",
+                    cd[i].t, cd[i].sv
+                );
+            }
+        }
+        if j != 4 {
+            Err(Error::BancroftError)
+        } else {
+            Ok(Self {
+                a,
+                b,
+                m,
+                ones: Self::one_vector(),
+            })
         }
-        Ok(Self {
-            a,
-            b,
-            m,
-            ones: Self::one_vector(),
-        })
     }
     pub fn resolve(&self) -> Result<Vector4<f64>, Error> {
-        let _output = Vector4::<f64>::default();
         let b_inv = self.b.try_inverse().ok_or(Error::MatrixInversionError)?;
         let b_1 = b_inv * self.ones;
         let b_a = b_inv * self.a;

src/bias/iono.rs

@@ -38,7 +38,7 @@ impl KbModel {
         const LAMBDA_P: f64 = 291.0;
         const L1_F: f64 = 1575.42E6;
 
-        let (phi_u, lambda_u) = rtm.apriori_rad;
+        let (phi_u, lambda_u) = rtm.rx_rad;
         let fract = R_EARTH / (R_EARTH + self.h_km);
         let (elev_rad, azim_rad) = (rtm.elevation_rad, rtm.azimuth_rad);
 
@@ -125,7 +125,7 @@ impl IonoComponents {
             Self::KbModel(model) => model.value(rtm),
             Self::NgModel(model) => model.value(rtm),
             Self::BdModel(model) => model.value(rtm),
-            Self::Stec(stec) => 0.0, //TODO
+            Self::Stec(..) => 0.0, //TODO
         }
     }
 }

src/bias/mod.rs

@@ -9,10 +9,9 @@ pub use iono::{BdModel, IonoComponents, IonosphereBias, KbModel, NgModel};
 pub(crate) struct RuntimeParams {
     pub t: Epoch,
     pub frequency: f64,
-    pub azimuth_deg: f64,
     pub elevation_deg: f64,
     pub azimuth_rad: f64,
     pub elevation_rad: f64,
-    pub apriori_geo: (f64, f64, f64),
-    pub apriori_rad: (f64, f64),
+    pub rx_geo: (f64, f64, f64),
+    pub rx_rad: (f64, f64),
 }

src/bias/tropo.rs

@@ -58,7 +58,7 @@ impl TropoComponents {
         const G_M: f64 = 9.784_f64;
 
         let day_of_year = rtm.t.day_of_year();
-        let (lat_ddeg, _, h) = rtm.apriori_geo;
+        let (lat_ddeg, _, h) = rtm.rx_geo;
 
         let mut lat = 15.0_f64;
         let mut min_delta = 180.0_f64;
@@ -164,7 +164,7 @@ impl TropoComponents {
     fn niel_model(prm: &RuntimeParams) -> f64 {
         const NS: f64 = 324.8;
 
-        let (_, _, h) = prm.apriori_geo;
+        let (_, _, h) = prm.rx_geo;
         let elev_rad = prm.elevation_rad;
         let h_km = h / 1000.0;