Skip to content

Commit

Permalink
Add one debug message at signal travel time estimation (#5)
Browse files Browse the repository at this point in the history 
Signed-off-by: Guillaume W. Bres <[email protected]>
  • Loading branch information
@gwbres
gwbres authored Mar 28, 2024
1 parent 95496c9 commit cb00dbd
Show file tree
Hide file tree
Showing 4 changed files with 116 additions and 87 deletions.
2 changes: 1 addition & 1 deletion Cargo.toml
View file
Original file line number Diff line number Diff line change
@@ -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 <[email protected]>"]
description = "GNSS position solver"
Expand Down
39 changes: 27 additions & 12 deletions src/candidate.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,23 +85,41 @@ 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::<f64>::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
*/
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
Expand All @@ -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
Expand Down Expand Up @@ -271,9 +289,6 @@ impl Candidate {
let e = (r_sun - r_sat) / norm;
let j = Vector3::<f64>::new(k[0] * e[0], k[1] * e[1], k[2] * e[2]);
let i = Vector3::<f64>::new(j[0] * k[0], j[1] * k[1], j[2] * k[2]);
let _r = Matrix3::<f64>::new(
i[0], j[0], k[0], i[1], j[1], k[1], i[2], j[2], k[2],
);
let r_dot = Vector3::<f64>::new(
(i[0] + j[0] + k[0]) * delta_apc,
(i[1] + j[1] + k[1]) * delta_apc,
Expand Down
6 changes: 3 additions & 3 deletions src/cfg.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,14 +137,14 @@ fn default_sv_apc() -> bool {
}

fn default_weight_matrix() -> Option<WeightMatrix> {
//Some(WeightMatrix::WeightMatrixMappingFunction(
None
//Some(WeightMatrix::MappingFunction(
// ElevationMappingFunction {
// a: 5.0,
// b: 0.0,
// c: 10.0,
// },
//))
None
}

fn default_filter_opts() -> Option<FilterOpts> {
Expand Down Expand Up @@ -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
Expand Down
156 changes: 85 additions & 71 deletions src/solver.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -312,71 +312,85 @@ impl<
let mut pool: Vec<Candidate> = 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::<f64>::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::<f64>::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::<f64>::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::<f64>::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();
Expand Down Expand Up @@ -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 {
Expand All @@ -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;
Expand Down

0 comments on commit cb00dbd

Please sign in to comment.