Develop (#191)

* corrected dop calculations * plot brdc clock corrections * take sp3 into account in clock state too * apc correction * bump rtk to v0.4.0 * bump cggtts to v4.1.0 --------- Signed-off-by: Guillaume W. Bres <[email protected]>
georust · Dec 4, 2023 · e34d637 · e34d637
1 parent b791eb0
commit e34d637
Show file tree

Hide file tree

Showing 6 changed files with 128 additions and 18 deletions.
diff --git a/rinex-cli/Cargo.toml b/rinex-cli/Cargo.toml
@@ -41,6 +41,6 @@ plotly =  "0.8.4"
 # plotly = { git = "https://github.com/gwbres/plotly", branch = "density-mapbox" }
 
 # solver
-gnss-rtk = { version = "0.3.1", features = ["serde"] }
+gnss-rtk = { version = "0.4.0", features = ["serde"] }
 # gnss-rtk = { git = "https://github.com/rtk-rs/gnss-rtk", branch = "develop", features = ["serde"] }
 # gnss-rtk = { path = "../../rtk-rs/gnss-rtk", features = ["serde"] }
diff --git a/rinex-cli/src/plot/record/navigation.rs b/rinex-cli/src/plot/record/navigation.rs
@@ -1,5 +1,6 @@
 use crate::plot::{build_3d_chart_epoch_label, build_chart_epoch_axis, PlotContext};
 use plotly::common::{Mode, Visible};
+use rinex::navigation::Ephemeris;
 use rinex::prelude::*;
 
 pub fn plot_navigation(ctx: &RnxContext, plot_ctx: &mut PlotContext) {
@@ -110,6 +111,7 @@ pub fn plot_navigation(ctx: &RnxContext, plot_ctx: &mut PlotContext) {
                     "Clock Drift [s/s]",
                 );
                 trace!("sv clock plot");
+                clock_plot_created = true;
             }
 
             let epochs: Vec<_> = sp3
@@ -310,4 +312,54 @@ pub fn plot_navigation(ctx: &RnxContext, plot_ctx: &mut PlotContext) {
             plot_ctx.add_trace(trace);
         }
     }
+    /*
+     * Plot BRDC Clock correction
+     */
+    if let Some(navdata) = ctx.nav_data() {
+        if let Some(obsdata) = ctx.obs_data() {
+            for (sv_index, sv) in obsdata.sv().enumerate() {
+                if sv_index == 0 {
+                    plot_ctx.add_cartesian2d_plot("SV Clock Correction", "Correction [s]");
+                    trace!("sv clock correction plot");
+                }
+                let epochs: Vec<_> = obsdata
+                    .observation()
+                    .filter_map(|((t, flag), (_, vehicles))| {
+                        if flag.is_ok() && vehicles.contains_key(&sv) {
+                            Some(*t)
+                        } else {
+                            None
+                        }
+                    })
+                    .collect();
+                let clock_corr: Vec<_> = obsdata
+                    .observation()
+                    .filter_map(|((t, flag), (_, vehicles))| {
+                        if flag.is_ok() {
+                            let (toe, sv_eph) = navdata.sv_ephemeris(sv, *t)?;
+                            /*
+                             * TODO prefer SP3 (if any)
+                             */
+                            let clock_state = sv_eph.sv_clock();
+                            let clock_corr = Ephemeris::sv_clock_corr(sv, clock_state, *t, toe);
+                            Some(clock_corr.to_seconds())
+                        } else {
+                            None
+                        }
+                    })
+                    .collect();
+
+                let trace =
+                    build_chart_epoch_axis(&format!("{}", sv), Mode::Markers, epochs, clock_corr)
+                        .visible({
+                            if sv_index < 3 {
+                                Visible::True
+                            } else {
+                                Visible::LegendOnly
+                            }
+                        });
+                plot_ctx.add_trace(trace);
+            }
+        }
+    }
 }
diff --git a/rinex-cli/src/positioning/post_process.rs b/rinex-cli/src/positioning/post_process.rs
@@ -169,15 +169,21 @@ pub fn post_process(
             "hdop",
             Mode::Markers,
             epochs.clone(),
-            results.values().map(|e| e.hdop()).collect::<Vec<f64>>(),
+            results
+                .values()
+                .map(|e| e.hdop(lat_ddeg, lon_ddeg))
+                .collect::<Vec<f64>>(),
         );
         plot_ctx.add_trace(trace);
 
         let trace = build_chart_epoch_axis(
             "vdop",
             Mode::Markers,
             epochs.clone(),
-            results.values().map(|e| e.vdop()).collect::<Vec<f64>>(),
+            results
+                .values()
+                .map(|e| e.vdop(lat_ddeg, lon_ddeg))
+                .collect::<Vec<f64>>(),
         )
         .y_axis("y2");
         plot_ctx.add_trace(trace);
@@ -227,7 +233,11 @@ pub fn post_process(
     for (epoch, solution) in results {
         let (px, py, pz) = (x + solution.p.x, y + solution.p.y, z + solution.p.z);
         let (lat, lon, alt) = map_3d::ecef2geodetic(px, py, pz, map_3d::Ellipsoid::WGS84);
-        let (hdop, vdop, tdop) = (solution.hdop(), solution.vdop(), solution.tdop());
+        let (hdop, vdop, tdop) = (
+            solution.hdop(lat_ddeg, lon_ddeg),
+            solution.vdop(lat_ddeg, lon_ddeg),
+            solution.tdop(),
+        );
         writeln!(
             fd,
             "{:?}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}",

diff --git a/rinex-cli/src/positioning/solver.rs b/rinex-cli/src/positioning/solver.rs
@@ -7,9 +7,9 @@ use rinex::navigation::Ephemeris;
 use rinex::prelude::{Observable, Rinex, RnxContext};
 
 use rtk::prelude::{
-    AprioriPosition, BdModel, Candidate, Config, Duration, Epoch, InterpolationResult,
-    IonosphericBias, KbModel, Mode, NgModel, Observation, PVTSolution, PVTSolutionType, Solver,
-    TroposphericBias, Vector3,
+    AprioriPosition, BdModel, Candidate, Config, Duration, Epoch, InterpolatedPosition,
+    InterpolationResult, IonosphericBias, KbModel, Mode, NgModel, Observation, PVTSolution,
+    PVTSolutionType, Solver, TroposphericBias, Vector3,
 };
 
 use map_3d::{ecef2geodetic, Ellipsoid};
@@ -188,6 +188,11 @@ pub fn solver(ctx: &mut RnxContext, cli: &Cli) -> Result<BTreeMap<Epoch, PVTSolu
     };
 
     let sp3_data = ctx.sp3_data();
+    let sp3_has_clock = match sp3_data {
+        Some(sp3) => sp3.sv_clock().count() > 0,
+        None => false,
+    };
+
     let meteo_data = ctx.meteo_data();
 
     let mut solver = Solver::new(
@@ -205,8 +210,8 @@ pub fn solver(ctx: &mut RnxContext, cli: &Cli) -> Result<BTreeMap<Epoch, PVTSolu
                     Some(InterpolationResult {
                         azimuth,
                         elevation,
-                        position: Vector3::new(x, y, z),
                         velocity: None,
+                        position: InterpolatedPosition::MassCenter(Vector3::new(x, y, z)),
                     })
                 } else {
                     // debug!("{:?} ({}): sp3 interpolation failed", t, sv);
@@ -217,8 +222,10 @@ pub fn solver(ctx: &mut RnxContext, cli: &Cli) -> Result<BTreeMap<Epoch, PVTSolu
                         Some(InterpolationResult {
                             azimuth,
                             elevation,
-                            position: Vector3::new(x, y, z),
                             velocity: None,
+                            position: InterpolatedPosition::AntennaPhaseCenter(Vector3::new(
+                                x, y, z,
+                            )),
                         })
                     } else {
                         // debug!("{:?} ({}): nav interpolation failed", t, sv);
@@ -233,7 +240,7 @@ pub fn solver(ctx: &mut RnxContext, cli: &Cli) -> Result<BTreeMap<Epoch, PVTSolu
                     Some(InterpolationResult {
                         azimuth,
                         elevation,
-                        position: Vector3::new(x, y, z),
+                        position: InterpolatedPosition::AntennaPhaseCenter(Vector3::new(x, y, z)),
                         velocity: None,
                     })
                 } else {
@@ -274,9 +281,47 @@ pub fn solver(ctx: &mut RnxContext, cli: &Cli) -> Result<BTreeMap<Epoch, PVTSolu
 
             let (toe, sv_eph) = sv_eph.unwrap();
 
-            let clock_state = sv_eph.sv_clock();
-            let clock_corr = Ephemeris::sv_clock_corr(*sv, clock_state, *t, toe);
-            let clock_state = Vector3::new(clock_state.0, clock_state.1, clock_state.2);
+            /*
+             * Prefer SP3 for clock state (if any),
+             * otherwise, use brdc
+             */
+            let clock_state = match sp3_has_clock {
+                true => {
+                    let sp3 = sp3_data.unwrap();
+                    if let Some(clk) = sp3
+                        .sv_clock()
+                        .filter_map(|(sp3_t, sp3_sv, clk)| {
+                            if sp3_t == *t && sp3_sv == *sv {
+                                Some(clk * 1.0E-6)
+                            } else {
+                                None
+                            }
+                        })
+                        .reduce(|clk, _| clk)
+                    {
+                        let clock_state = sv_eph.sv_clock();
+                        Vector3::new(clock_state.0, 0.0_f64, 0.0_f64)
+                    } else {
+                        /*
+                         * SP3 preference: abort on missing Epochs
+                         */
+                        //continue ;
+                        let clock_state = sv_eph.sv_clock();
+                        Vector3::new(clock_state.0, clock_state.1, clock_state.2)
+                    }
+                },
+                false => {
+                    let clock_state = sv_eph.sv_clock();
+                    Vector3::new(clock_state.0, clock_state.1, clock_state.2)
+                },
+            };
+
+            let clock_corr = Ephemeris::sv_clock_corr(
+                *sv,
+                (clock_state[0], clock_state[1], clock_state[2]),
+                *t,
+                toe,
+            );
 
             let mut codes = Vec::<Observation>::new();
             let mut phases = Vec::<Observation>::new();

diff --git a/rnx2cggtts/Cargo.toml b/rnx2cggtts/Cargo.toml
@@ -26,11 +26,11 @@ serde = { version = "1.0", default-features = false, features = ["derive"] }
 rinex = { path = "../rinex", version = "=0.15.1", features = ["full"] }
 
 # cggtts
-cggtts = { version = "4.0.1", features = ["serde", "scheduler"] }
+cggtts = { version = "4.1.0", features = ["serde", "scheduler"] }
 # cggtts = { git = "https://github.com/gwbres/cggtts", branch = "develop", features = ["serde", "scheduler"] }
 # cggtts = { path = "../../cggtts/cggtts", features = ["serde", "scheduler"] }
 
 # solver
-gnss-rtk = { version = "0.3.1", features = ["serde"] }
+gnss-rtk = { version = "0.4.0", features = ["serde"] }
 # gnss-rtk = { git = "https://github.com/rtk-rs/gnss-rtk", branch = "develop", features = ["serde"] }
 # gnss-rtk = { path = "../../rtk-rs/gnss-rtk", features = ["serde"] }
diff --git a/rnx2cggtts/src/solver.rs b/rnx2cggtts/src/solver.rs
@@ -18,6 +18,7 @@ use rtk::prelude::{
     Config,
     Duration,
     Epoch,
+    InterpolatedPosition,
     InterpolationResult,
     IonosphericBias,
     KbModel,
@@ -251,8 +252,8 @@ pub fn resolve(ctx: &mut RnxContext, cli: &Cli) -> Result<Vec<Track>, Error> {
                     Some(InterpolationResult {
                         azimuth,
                         elevation,
-                        position: Vector3::new(x, y, z),
                         velocity: None,
+                        position: InterpolatedPosition::MassCenter(Vector3::new(x, y, z)),
                     })
                 } else {
                     // debug!("{:?} ({}): sp3 interpolation failed", t, sv);
@@ -263,8 +264,10 @@ pub fn resolve(ctx: &mut RnxContext, cli: &Cli) -> Result<Vec<Track>, Error> {
                         Some(InterpolationResult {
                             azimuth,
                             elevation,
-                            position: Vector3::new(x, y, z),
                             velocity: None,
+                            position: InterpolatedPosition::AntennaPhaseCenter(Vector3::new(
+                                x, y, z,
+                            )),
                         })
                     } else {
                         // debug!("{:?} ({}): nav interpolation failed", t, sv);
@@ -279,8 +282,8 @@ pub fn resolve(ctx: &mut RnxContext, cli: &Cli) -> Result<Vec<Track>, Error> {
                     Some(InterpolationResult {
                         azimuth,
                         elevation,
-                        position: Vector3::new(x, y, z),
                         velocity: None,
+                        position: InterpolatedPosition::AntennaPhaseCenter(Vector3::new(x, y, z)),
                     })
                 } else {
                     // debug!("{:?} ({}): nav interpolation failed", t, sv);