Skip to content

Commit

Permalink
working on rtk solver
Browse files Browse the repository at this point in the history 
Signed-off-by: Guillaume W. Bres <[email protected]>
  • Loading branch information
@gwbres
gwbres committed Sep 27, 2023
1 parent 9805ac8 commit 03af6f9
Show file tree
Hide file tree
Showing 5 changed files with 147 additions and 65 deletions.
4 changes: 2 additions & 2 deletions gnss-rtk/src/estimate.rs
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
use nalgebra::base::{Matrix4xX, Vector4};
use nalgebra::base::{DVector, Matrix4xX, Vector4};
/*
* Solver solution estimate
* is always expressed as a correction of an 'a priori' position
Expand All @@ -25,7 +25,7 @@ impl SolverEstimate {
* Builds a new SolverEstimate from `g` Nav Matrix,
* and `y` Nav Vector
*/
pub fn new(g: Matrix4xX<f64>, y: Vector4<f64>) -> Option<Self> {
pub fn new(g: Matrix4xX<f64>, y: DVector<f64>) -> Option<Self> {
let g_prime = g.transpose();
let q = (g.clone() * g_prime.clone()).try_inverse()?;
let x = q * g_prime.clone() * y;
Expand Down
117 changes: 85 additions & 32 deletions gnss-rtk/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ use hifitime::Unit;

extern crate nyx_space as nyx;

use nalgebra::base::{Matrix4xX, Vector3, Vector4};
use nalgebra::base::{DVector, Matrix4xX, Vector1, Vector3, Vector4};
use nyx::md::prelude::{Arc, Cosm};

mod estimate;
Expand Down Expand Up @@ -38,6 +38,8 @@ use thiserror::Error;
pub enum SolverError {
#[error("provided context is either unsufficient or invalid for any position solving")]
Unfeasible,
#[error("apriori position is not defined - initialization it not complete!")]
UndefinedAprioriPosition,
#[error("failed to initialize solver - \"{0}\"")]
InitializationError(String),
#[error("no vehicles elected @{0}")]
Expand Down Expand Up @@ -158,18 +160,27 @@ impl Solver {
return Err(SolverError::NotInitialized);
}

let pos0 = self
.opts
.rcvr_position
.ok_or(SolverError::UndefinedAprioriPosition)?;

let (x0, y0, z0): (f64, f64, f64) = pos0.into();

let modeling = self.opts.modeling;

// grab work instant
let t = ctx
.primary_data()
.epoch()
.nth(self.nth_epoch)
.ok_or(SolverError::EpochDetermination(self.nth_epoch))?;
let t = ctx.primary_data().epoch().nth(self.nth_epoch);

if t.is_none() {
self.nth_epoch += 1;
return Err(SolverError::EpochDetermination(self.nth_epoch));
}

let t = t.unwrap();
let interp_order = self.opts.interp_order;

/* elect vehicles */
/* selection */
let elected_sv = Self::sv_election(ctx, t, &self.opts);
if elected_sv.is_none() {
warn!("no vehicles elected @ {}", t);
Expand All @@ -186,9 +197,8 @@ impl Solver {
return Err(SolverError::LessThan4Sv(t));
}

/* determine sv positions */
/* 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() {
Expand All @@ -212,7 +222,6 @@ impl Solver {
}
}
}

/* remove sv in eclipse */
if self.solver == SolverType::PPP {
if let Some(min_rate) = self.opts.min_sv_sunlight_rate {
Expand All @@ -234,16 +243,27 @@ impl Solver {
});
}
}

// 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, modeling) {
t_tx.insert(*sv, sv_t_tx);
}
}
// 4: retrieve rt pseudorange
let pr: Vec<_> = ctx
.primary_data()
.pseudo_range()
.filter_map(|((epoch, _), sv, _, pr)| {
if epoch == t && elected_sv.contains(&sv) {
Some((sv, pr))
} else {
None
}
})
.collect();

// 4: position @ tx
// 5: position @ tx
let mut sv_pos: HashMap<Sv, Vector3<f64>> = HashMap::new();
for t_tx in t_tx {
if modeling.relativistic_clock_corr {
Expand All @@ -257,21 +277,52 @@ impl Solver {
}
}

// 5: pseudo range
let mut pr: HashMap<Sv, f64> = HashMap::new();
for (sv, pos) in sv_pos {
let pseudorange = (pos[0]).sqrt();
pr.insert(sv, pseudorange);
// 6: form matrix
let mut y = DVector::<f64>::zeros(elected_sv.len());
let mut g = Matrix4xX::<f64>::zeros(elected_sv.len());

for (index, (sv, pos)) in sv_pos.iter().enumerate() {
let rho_0 =
((pos[0] - x0).powi(2) + (pos[1] - y0).powi(2) + (pos[2] - z0).powi(2)).sqrt();

//TODO
//let models = models
// .iter()
// .filter_map(|sv, model| {
// if sv == svnn {
// Some(model)
// } else {

// }
// })
// .reduce(|m, _| m)
// .unwrap();
let models = 0.0_f64;

let pr = pr
.iter()
.filter_map(|(svnn, pr)| if sv == svnn { Some(pr) } else { None })
.reduce(|pr, _| pr)
.unwrap();

y[index] = pr - rho_0 - models;

let (x, y, z) = (pos[0], pos[1], pos[2]);

g[(index, 0)] = (x0 - x) / rho_0;
g[(index, 1)] = (y0 - y) / rho_0;
g[(index, 2)] = (z0 - z) / rho_0;
g[(index, 3)] = 1.0_f64;
}

// 5: form matrix
let g = Matrix4xX::<f64>::zeros(elected_sv.len());
let y = Vector4::<f64>::zeros();

// 6: resolve
let estimate = SolverEstimate::new(g, y).ok_or(SolverError::SolvingError(t))?;

Ok((t, estimate))
// 7: resolve
let estimate = SolverEstimate::new(g, y);
if estimate.is_none() {
self.nth_epoch += 1;
return Err(SolverError::SolvingError(t));
} else {
Ok((t, estimate.unwrap()))
}
}
/*
* Evalutes T_tx transmission time, for given Sv at desired 't'
Expand All @@ -293,24 +344,23 @@ impl Solver {
if let Some(pr) = pr.next() {
let t_tx = Duration::from_seconds(t.to_duration().to_seconds() - pr / SPEED_OF_LIGHT);
let mut e_tx = Epoch::from_duration(t_tx, sv.constellation.timescale()?);
debug!("t_tx(pr): {}@{} : {}", sv, t, t_tx);

if m.sv_clock_bias {
let dt_sat = nav.sv_clock_bias(sv, e_tx)?;
debug!("clock bias: {}@{} : {}", sv, t, dt_sat);

debug!("{}@{} | dt_sat {}", sv, t, dt_sat);
e_tx -= dt_sat;
debug!("{} : t(obs): {} | t(tx) {}", sv, t, e_tx);
}

if m.sv_total_group_delay {
if let Some(nav) = ctx.navigation_data() {
if let Some(tgd) = nav.sv_tgd(sv, t) {
e_tx = e_tx - tgd * Unit::Second;
let tgd = tgd * Unit::Second;
debug!("{}@{} | tgd {}", sv, t, tgd);
e_tx = e_tx - tgd;
}
}
}

debug!("{}@{} | t_tx {}", sv, t, e_tx);
Some(e_tx)
} else {
debug!("missing PR measurement");
Expand Down Expand Up @@ -353,6 +403,9 @@ impl Solver {
* Elects sv for this epoch
*/
fn sv_election(ctx: &QcContext, t: Epoch, opts: &SolverOpts) -> Option<Vec<Sv>> {
const max_sv: usize = 5;
//TODO: make sure pseudo range exists
//TODO: make sure context is consistent with solving strategy : SPP / PPP
ctx.primary_data()
.sv_epoch()
.filter_map(|(epoch, svs)| {
Expand All @@ -363,7 +416,7 @@ impl Solver {
// .count()
//} else {
// no gnss filter / criteria
Some(svs)
Some(svs.into_iter().take(max_sv).collect())
//}
} else {
None
Expand Down
71 changes: 50 additions & 21 deletions rinex-cli/src/cli.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@ impl Cli {
.long("fp")
.value_name("FILE")
.help("Input RINEX file. Serves as primary data.
In advanced usage, this must be Observation Data.
Observation, Meteo and IONEX, can only serve as primary data.")
Must be Observation Data for --rtk.
Observation, Meteo and IONEX can only serve as primary data.")
.action(ArgAction::Append)
.required(true))
.next_help_heading("General")
Expand All @@ -37,8 +37,9 @@ Observation, Meteo and IONEX, can only serve as primary data.")
.long("quiet")
.action(ArgAction::SetTrue)
.help("Disable all terminal output. Also disables auto HTML reports opener."))
.arg(Arg::new("readable")
.short('r')
.arg(Arg::new("pretty")
.short('p')
.long("pretty")
.action(ArgAction::SetTrue)
.help("Make terminal output more readable."))
.arg(Arg::new("workspace")
Expand Down Expand Up @@ -109,6 +110,7 @@ Useful to determine common Epochs or compare sample rates in between
.num_args(1..)
.help("Design preprocessing operations, like data filtering or resampling,
prior further analysis. You can stack as many ops as you need.
Preprocessing ops apply prior entering both -q and --rtk modes.
Refer to rinex-cli/doc/preprocessing.md to learn how to operate this interface."))
.next_help_heading("Observation RINEX")
.arg(Arg::new("observables")
Expand Down Expand Up @@ -250,27 +252,54 @@ The summary report by default is integrated to the global HTML report."))
.long("qc-only")
.action(ArgAction::SetTrue)
.help("Activates QC mode and disables all other features: quickest qc rendition."))
.next_help_heading("Position Solver")
.arg(Arg::new("positioning")
.short('p')
.long("positioning")
.next_help_heading("RTK (Positioning)")
.arg(Arg::new("rtk")
.short('r')
.long("rtk")
.action(ArgAction::SetTrue)
.help("Activate GNSS receiver position solver.
This is only possible if provided context is sufficient.
Depending on provided context, either SPP (high accuracy) or PPP (ultra high accuracy)
method is deployed.
This is turned of by default, because it involves quite heavy computations.
solver is deployed.
This mode is turned off by default because it involves quite heavy computations.
Use the RUST_LOG env. variable for verbosity.
See [spp] for more information. "))
.arg(Arg::new("spp")
.long("spp")
.action(ArgAction::SetTrue)
.help("Enables Positioning forced to Single Frequency SPP solver mode.
Disregards whether the provided context is PPP compatible.
NB: we do not account for Relativistic effects in clock bias estimates."))
.arg(Arg::new("positioning-only")
.long("pos-only")
.action(ArgAction::SetTrue)
.help("Activates GNSS position solver, disables all other modes: most performant solver."))
NB: we do not account for Relativistic effects by default and raw pseudo range are used.
For indepth customization, refer to the configuration file and online documentation."))
.arg(Arg::new("rtk-only")
.long("rtk-only")
.action(ArgAction::SetTrue)
.help("Activates GNSS position solver, disables all other modes.
This is the most performant mode to solve a position."))
.arg(Arg::new("rtk-fixed-altitude")
.long("rtk-fixed-alt")
.value_name("ALTITUDE(f64)")
.help("Set rtk solver to fixed altitude mode.
Problem is simplified by not caring about the Z axis resolution."))
.arg(Arg::new("rtk-static")
.long("rtk-static")
.help("Set rtk solver to static mode.
Problem is simplified but will not work in case the receiver is not maintained in static position.
Works well in laboratory conditions.
Combine --rtk-fixed-alt --rtk-static is most efficient solving scenario."))
.arg(Arg::new("rtk-model")
.long("rtk-model")
.action(ArgAction::Append)
.help("Stack one modelization to account for when solving.
--model=tgd : account for SV total group delay
--model=smoothing: smooth pseudo ranges. This is pointless if you requested
the hatch filter with -P.
--model=eclipse:f64 : adjust minimal light rate to determine eclipse condition.
--model=tgd : account for SV total group delay"))
.arg(Arg::new("kml")
.long("kml")
.help("Form a KML track with resolved positions.
This turns off the default visualization."))
.next_help_heading("File operations")
.arg(Arg::new("merge")
.short('m')
Expand Down Expand Up @@ -435,16 +464,16 @@ Refer to README"))
self.matches.get_flag(flag)
}
/// returns true if pretty JSON is requested
pub fn readable_json(&self) -> bool {
self.get_flag("readable")
pub fn pretty(&self) -> bool {
self.get_flag("pretty")
}
/// Returns true if quiet mode is activated
pub fn quiet(&self) -> bool {
self.matches.get_flag("quiet")
}
/// Returns true if position solver is enabled
pub fn positioning(&self) -> bool {
self.matches.get_flag("positioning") || self.forced_spp() || self.forced_ppp()
pub fn rtk(&self) -> bool {
self.matches.get_flag("rtk") || self.forced_spp() || self.forced_ppp()
}
/// Returns true if position solver forced to SPP
pub fn forced_spp(&self) -> bool {
Expand All @@ -454,8 +483,8 @@ Refer to README"))
pub fn forced_ppp(&self) -> bool {
self.matches.get_flag("spp")
}
pub fn positioning_only(&self) -> bool {
self.matches.get_flag("positioning-only")
pub fn rtk_only(&self) -> bool {
self.matches.get_flag("rtk-only")
}
pub fn cs_graph(&self) -> bool {
self.matches.get_flag("cs")
Expand Down
2 changes: 1 addition & 1 deletion rinex-cli/src/identification.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ use rinex_qc::QcContext;
* Basic identification operations
*/
pub fn rinex_identification(ctx: &QcContext, cli: &Cli) {
let pretty = cli.readable_json();
let pretty = cli.pretty();
let ops = cli.identification_ops();

identification(&ctx.primary_data(), pretty, ops.clone());
Expand Down
Loading

0 comments on commit 03af6f9

Please sign in to comment.