diff --git a/Cargo.toml b/Cargo.toml index 4625e60..681d1c4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "gnss-rtk" -version = "0.4.2" +version = "0.4.3" license = "MIT OR Apache-2.0" authors = ["Guillaume W. Bres "] description = "GNSS position solver" diff --git a/src/candidate.rs b/src/candidate.rs index 21c8b5d..cf5424b 100644 --- a/src/candidate.rs +++ b/src/candidate.rs @@ -85,15 +85,33 @@ impl Candidate { } } /* - * Returns one pseudo range observation [m], disregarding its frequency. - * Infaillible, because we don't allow to build Self without at least - * 1 PR observation + * Returns one pseudo range observation [m], whatever the frequency. + * Best SNR is preferred though (if such information was provided). */ pub(crate) fn prefered_pseudorange(&self) -> Option<&Observation> { - self.code - .iter() - // .map(|pr| pr.value) - .reduce(|k, _| k) + let mut snr = Option::::None; + let mut code = Option::<&Observation>::None; + for c in &self.code { + if code.is_none() { + code = Some(c); + snr = c.snr; + } else { + // prefer best SNR if possible + if let Some(s1) = c.snr { + if snr.is_some() { + let s2 = snr.unwrap(); + if s1 > s2 { + snr = Some(s1); + code = Some(c); + } + } else { + snr = Some(s1); + code = Some(c); + } + } + } + } + code } /* * Returns true if we have pseudo range observ on two carriers @@ -101,7 +119,7 @@ impl Candidate { pub(crate) fn dual_freq_pseudorange(&self) -> bool { self.code .iter() - .map(|c| (c.frequency * 10.0) as u16) + .map(|c| (c.frequency * 100.0) as u16) .unique() .count() > 0 @@ -112,7 +130,7 @@ impl Candidate { pub(crate) fn dual_freq_phase(&self) -> bool { self.phase .iter() - .map(|c| (c.frequency * 10.0) as u16) + .map(|c| (c.frequency * 100.0) as u16) .unique() .count() > 0 @@ -271,9 +289,6 @@ impl Candidate { let e = (r_sun - r_sat) / norm; let j = Vector3::::new(k[0] * e[0], k[1] * e[1], k[2] * e[2]); let i = Vector3::::new(j[0] * k[0], j[1] * k[1], j[2] * k[2]); - let _r = Matrix3::::new( - i[0], j[0], k[0], i[1], j[1], k[1], i[2], j[2], k[2], - ); let r_dot = Vector3::::new( (i[0] + j[0] + k[0]) * delta_apc, (i[1] + j[1] + k[1]) * delta_apc, diff --git a/src/cfg.rs b/src/cfg.rs index ff01b82..cf5f420 100644 --- a/src/cfg.rs +++ b/src/cfg.rs @@ -137,14 +137,14 @@ fn default_sv_apc() -> bool { } fn default_weight_matrix() -> Option { - //Some(WeightMatrix::WeightMatrixMappingFunction( + None + //Some(WeightMatrix::MappingFunction( // ElevationMappingFunction { // a: 5.0, // b: 0.0, // c: 10.0, // }, //)) - None } fn default_filter_opts() -> Option { @@ -267,7 +267,7 @@ impl Modeling { } } -#[derive(Default, Debug, Clone, PartialEq)] +#[derive(Debug, Clone, PartialEq)] #[cfg_attr(feature = "serde", derive(Deserialize))] pub struct Config { /// Time scale diff --git a/src/solver.rs b/src/solver.rs index ae523a5..9ec94d4 100644 --- a/src/solver.rs +++ b/src/solver.rs @@ -312,71 +312,85 @@ impl< let mut pool: Vec = pool .iter() .filter_map(|c| { - let (t_tx, dt_ttx) = c.transmission_time(&self.cfg).ok()?; - - if let Some(mut interpolated) = (self.interpolator)(t_tx, c.sv, interp_order) { - let mut c = c.clone(); - - let rot = match modeling.earth_rotation { - true => { - const EARTH_OMEGA_E_WGS84: f64 = 7.2921151467E-5; - let we = EARTH_OMEGA_E_WGS84 * dt_ttx; - let (we_cos, we_sin) = (we.cos(), we.sin()); - Matrix3::::new( - we_cos, we_sin, 0.0_f64, -we_sin, we_cos, 0.0_f64, 0.0_f64, - 0.0_f64, 1.0_f64, - ) - }, - false => Matrix3::::new( - 1.0_f64, 0.0_f64, 0.0_f64, 0.0_f64, 1.0_f64, 0.0_f64, 0.0_f64, 0.0_f64, - 1.0_f64, - ), - }; - - interpolated.position = - InterpolatedPosition::AntennaPhaseCenter(rot * interpolated.position()); - - /* determine velocity */ - if let Some((p_ttx, p_position)) = self.prev_sv_state.get(&c.sv) { - let dt = (t_tx - *p_ttx).to_seconds(); - interpolated.velocity = - Some((rot * interpolated.position() - rot * p_position) / dt); - } + match c.transmission_time(&self.cfg) { + Ok((t_tx, dt_ttx)) => { + debug!("{:?} ({}) : signal travel time: {}", t_tx, c.sv, dt_ttx); + if let Some(mut interpolated) = + (self.interpolator)(t_tx, c.sv, interp_order) + { + let mut c = c.clone(); + + let rot = match modeling.earth_rotation { + true => { + const EARTH_OMEGA_E_WGS84: f64 = 7.2921151467E-5; + let we = EARTH_OMEGA_E_WGS84 * dt_ttx; + let (we_cos, we_sin) = (we.cos(), we.sin()); + Matrix3::::new( + we_cos, we_sin, 0.0_f64, -we_sin, we_cos, 0.0_f64, 0.0_f64, + 0.0_f64, 1.0_f64, + ) + }, + false => Matrix3::::new( + 1.0_f64, 0.0_f64, 0.0_f64, 0.0_f64, 1.0_f64, 0.0_f64, 0.0_f64, + 0.0_f64, 1.0_f64, + ), + }; + + interpolated.position = InterpolatedPosition::AntennaPhaseCenter( + rot * interpolated.position(), + ); - self.prev_sv_state - .insert(c.sv, (t_tx, interpolated.position())); - - if modeling.relativistic_clock_bias { - /* - * following calculations need inst. velocity - */ - if interpolated.velocity.is_some() { - let _orbit = interpolated.orbit(t_tx, self.earth_frame); - const EARTH_SEMI_MAJOR_AXIS_WGS84: f64 = 6378137.0_f64; - const EARTH_GRAVITATIONAL_CONST: f64 = 3986004.418 * 10.0E8; - let orbit = interpolated.orbit(t_tx, self.earth_frame); - let ea_rad = deg2rad(orbit.ea_deg()); - let gm = - (EARTH_SEMI_MAJOR_AXIS_WGS84 * EARTH_GRAVITATIONAL_CONST).sqrt(); - let bias = -2.0_f64 * orbit.ecc() * ea_rad.sin() * gm - / SPEED_OF_LIGHT - / SPEED_OF_LIGHT - * Unit::Second; - debug!("{:?} ({}) : relativistic clock bias: {}", t_tx, c.sv, bias); - c.clock_corr += bias; - } - } + /* determine velocity */ + if let Some((p_ttx, p_position)) = self.prev_sv_state.get(&c.sv) { + let dt = (t_tx - *p_ttx).to_seconds(); + interpolated.velocity = + Some((rot * interpolated.position() - rot * p_position) / dt); + } + + self.prev_sv_state + .insert(c.sv, (t_tx, interpolated.position())); + + if modeling.relativistic_clock_bias { + /* + * following calculations need inst. velocity + */ + if interpolated.velocity.is_some() { + let _orbit = interpolated.orbit(t_tx, self.earth_frame); + const EARTH_SEMI_MAJOR_AXIS_WGS84: f64 = 6378137.0_f64; + const EARTH_GRAVITATIONAL_CONST: f64 = 3986004.418 * 10.0E8; + let orbit = interpolated.orbit(t_tx, self.earth_frame); + let ea_rad = deg2rad(orbit.ea_deg()); + let gm = (EARTH_SEMI_MAJOR_AXIS_WGS84 + * EARTH_GRAVITATIONAL_CONST) + .sqrt(); + let bias = -2.0_f64 * orbit.ecc() * ea_rad.sin() * gm + / SPEED_OF_LIGHT + / SPEED_OF_LIGHT + * Unit::Second; + debug!( + "{:?} ({}) : relativistic clock bias: {}", + t_tx, c.sv, bias + ); + c.clock_corr += bias; + } + } - debug!( - "{:?} ({}) : interpolated state: {:?}", - t_tx, c.sv, interpolated.position - ); + debug!( + "{:?} ({}) : interpolated state: {:?}", + t_tx, c.sv, interpolated.position + ); - c.state = Some(interpolated); - Some(c) - } else { - warn!("{:?} ({}) : interpolation failed", t_tx, c.sv); - None + c.state = Some(interpolated); + Some(c) + } else { + warn!("{:?} ({}) : interpolation failed", t_tx, c.sv); + None + } + }, + Err(e) => { + error!("{} - transsmision time error: {:?}", c.sv, e); + None + }, } }) .collect(); @@ -519,8 +533,11 @@ impl< self.filter_state.clone(), )?; - if filter != Filter::None { - self.filter_state = new_state; + let validator = SolutionValidator::new(&self.apriori.ecef, &pool, &w, &pvt_solution); + + let valid = validator.valid(solver_opts); + if valid.is_err() { + return Err(Error::InvalidatedSolution(valid.err().unwrap())); } if let Some((prev_t, prev_pvt)) = &self.prev_pvt { @@ -529,16 +546,13 @@ impl< self.prev_pvt = Some((t, pvt_solution.clone())); - let validator = SolutionValidator::new(&self.apriori.ecef, &pool, &w, &pvt_solution); - - let valid = validator.valid(solver_opts); - if valid.is_err() { - return Err(Error::InvalidatedSolution(valid.err().unwrap())); + if filter != Filter::None { + self.filter_state = new_state; } /* - * slightly rework the solution so it ""physically"" (/ looks like) - * what we expect based on the predefined setup. + * slightly rework the solution so it ""looks"" like + * what we expect based on the defined setup. */ if let Some(alt) = self.cfg.fixed_altitude { pvt_solution.pos.z = self.apriori.ecef.z - alt;