Skip to content

Commit cb00dbd

Browse files
gwbresgwbres
authored
Add one debug message at signal travel time estimation (#5)
Signed-off-by: Guillaume W. Bres <guillaume.bressaix@gmail.com>
1 parent 95496c9 commit cb00dbd

File tree

4 files changed

+116
-87
lines changed

4 files changed

+116
-87
lines changed

‎Cargo.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
[package]
22
name = "gnss-rtk"
3-
version = "0.4.2"
3+
version = "0.4.3"
44
license = "MIT OR Apache-2.0"
55
authors = ["Guillaume W. Bres <guillaume.bressaix@gmail.com>"]
66
description = "GNSS position solver"

‎src/candidate.rs‎

Lines changed: 27 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -85,23 +85,41 @@ impl Candidate {
8585
}
8686
}
8787
/*
88-
* Returns one pseudo range observation [m], disregarding its frequency.
89-
* Infaillible, because we don't allow to build Self without at least
90-
* 1 PR observation
88+
* Returns one pseudo range observation [m], whatever the frequency.
89+
* Best SNR is preferred though (if such information was provided).
9190
*/
9291
pub(crate) fn prefered_pseudorange(&self) -> Option<&Observation> {
93-
self.code
94-
.iter()
95-
// .map(|pr| pr.value)
96-
.reduce(|k, _| k)
92+
let mut snr = Option::<f64>::None;
93+
let mut code = Option::<&Observation>::None;
94+
for c in &self.code {
95+
if code.is_none() {
96+
code = Some(c);
97+
snr = c.snr;
98+
} else {
99+
// prefer best SNR if possible
100+
if let Some(s1) = c.snr {
101+
if snr.is_some() {
102+
let s2 = snr.unwrap();
103+
if s1 > s2 {
104+
snr = Some(s1);
105+
code = Some(c);
106+
}
107+
} else {
108+
snr = Some(s1);
109+
code = Some(c);
110+
}
111+
}
112+
}
113+
}
114+
code
97115
}
98116
/*
99117
* Returns true if we have pseudo range observ on two carriers
100118
*/
101119
pub(crate) fn dual_freq_pseudorange(&self) -> bool {
102120
self.code
103121
.iter()
104-
.map(|c| (c.frequency * 10.0) as u16)
122+
.map(|c| (c.frequency * 100.0) as u16)
105123
.unique()
106124
.count()
107125
> 0
@@ -112,7 +130,7 @@ impl Candidate {
112130
pub(crate) fn dual_freq_phase(&self) -> bool {
113131
self.phase
114132
.iter()
115-
.map(|c| (c.frequency * 10.0) as u16)
133+
.map(|c| (c.frequency * 100.0) as u16)
116134
.unique()
117135
.count()
118136
> 0
@@ -271,9 +289,6 @@ impl Candidate {
271289
let e = (r_sun - r_sat) / norm;
272290
let j = Vector3::<f64>::new(k[0] * e[0], k[1] * e[1], k[2] * e[2]);
273291
let i = Vector3::<f64>::new(j[0] * k[0], j[1] * k[1], j[2] * k[2]);
274-
let _r = Matrix3::<f64>::new(
275-
i[0], j[0], k[0], i[1], j[1], k[1], i[2], j[2], k[2],
276-
);
277292
let r_dot = Vector3::<f64>::new(
278293
(i[0] + j[0] + k[0]) * delta_apc,
279294
(i[1] + j[1] + k[1]) * delta_apc,

‎src/cfg.rs‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -137,14 +137,14 @@ fn default_sv_apc() -> bool {
137137
}
138138

139139
fn default_weight_matrix() -> Option<WeightMatrix> {
140-
//Some(WeightMatrix::WeightMatrixMappingFunction(
140+
None
141+
//Some(WeightMatrix::MappingFunction(
141142
// ElevationMappingFunction {
142143
// a: 5.0,
143144
// b: 0.0,
144145
// c: 10.0,
145146
// },
146147
//))
147-
None
148148
}
149149

150150
fn default_filter_opts() -> Option<FilterOpts> {
@@ -267,7 +267,7 @@ impl Modeling {
267267
}
268268
}
269269

270-
#[derive(Default, Debug, Clone, PartialEq)]
270+
#[derive(Debug, Clone, PartialEq)]
271271
#[cfg_attr(feature = "serde", derive(Deserialize))]
272272
pub struct Config {
273273
/// Time scale

‎src/solver.rs‎

Lines changed: 85 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -312,71 +312,85 @@ impl<
312312
let mut pool: Vec<Candidate> = pool
313313
.iter()
314314
.filter_map(|c| {
315-
let (t_tx, dt_ttx) = c.transmission_time(&self.cfg).ok()?;
316-
317-
if let Some(mut interpolated) = (self.interpolator)(t_tx, c.sv, interp_order) {
318-
let mut c = c.clone();
319-
320-
let rot = match modeling.earth_rotation {
321-
true => {
322-
const EARTH_OMEGA_E_WGS84: f64 = 7.2921151467E-5;
323-
let we = EARTH_OMEGA_E_WGS84 * dt_ttx;
324-
let (we_cos, we_sin) = (we.cos(), we.sin());
325-
Matrix3::<f64>::new(
326-
we_cos, we_sin, 0.0_f64, -we_sin, we_cos, 0.0_f64, 0.0_f64,
327-
0.0_f64, 1.0_f64,
328-
)
329-
},
330-
false => Matrix3::<f64>::new(
331-
1.0_f64, 0.0_f64, 0.0_f64, 0.0_f64, 1.0_f64, 0.0_f64, 0.0_f64, 0.0_f64,
332-
1.0_f64,
333-
),
334-
};
335-
336-
interpolated.position =
337-
InterpolatedPosition::AntennaPhaseCenter(rot * interpolated.position());
338-
339-
/* determine velocity */
340-
if let Some((p_ttx, p_position)) = self.prev_sv_state.get(&c.sv) {
341-
let dt = (t_tx - *p_ttx).to_seconds();
342-
interpolated.velocity =
343-
Some((rot * interpolated.position() - rot * p_position) / dt);
344-
}
315+
match c.transmission_time(&self.cfg) {
316+
Ok((t_tx, dt_ttx)) => {
317+
debug!("{:?} ({}) : signal travel time: {}", t_tx, c.sv, dt_ttx);
318+
if let Some(mut interpolated) =
319+
(self.interpolator)(t_tx, c.sv, interp_order)
320+
{
321+
let mut c = c.clone();
322+
323+
let rot = match modeling.earth_rotation {
324+
true => {
325+
const EARTH_OMEGA_E_WGS84: f64 = 7.2921151467E-5;
326+
let we = EARTH_OMEGA_E_WGS84 * dt_ttx;
327+
let (we_cos, we_sin) = (we.cos(), we.sin());
328+
Matrix3::<f64>::new(
329+
we_cos, we_sin, 0.0_f64, -we_sin, we_cos, 0.0_f64, 0.0_f64,
330+
0.0_f64, 1.0_f64,
331+
)
332+
},
333+
false => Matrix3::<f64>::new(
334+
1.0_f64, 0.0_f64, 0.0_f64, 0.0_f64, 1.0_f64, 0.0_f64, 0.0_f64,
335+
0.0_f64, 1.0_f64,
336+
),
337+
};
338+
339+
interpolated.position = InterpolatedPosition::AntennaPhaseCenter(
340+
rot * interpolated.position(),
341+
);
345342

346-
self.prev_sv_state
347-
.insert(c.sv, (t_tx, interpolated.position()));
348-
349-
if modeling.relativistic_clock_bias {
350-
/*
351-
* following calculations need inst. velocity
352-
*/
353-
if interpolated.velocity.is_some() {
354-
let _orbit = interpolated.orbit(t_tx, self.earth_frame);
355-
const EARTH_SEMI_MAJOR_AXIS_WGS84: f64 = 6378137.0_f64;
356-
const EARTH_GRAVITATIONAL_CONST: f64 = 3986004.418 * 10.0E8;
357-
let orbit = interpolated.orbit(t_tx, self.earth_frame);
358-
let ea_rad = deg2rad(orbit.ea_deg());
359-
let gm =
360-
(EARTH_SEMI_MAJOR_AXIS_WGS84 * EARTH_GRAVITATIONAL_CONST).sqrt();
361-
let bias = -2.0_f64 * orbit.ecc() * ea_rad.sin() * gm
362-
/ SPEED_OF_LIGHT
363-
/ SPEED_OF_LIGHT
364-
* Unit::Second;
365-
debug!("{:?} ({}) : relativistic clock bias: {}", t_tx, c.sv, bias);
366-
c.clock_corr += bias;
367-
}
368-
}
343+
/* determine velocity */
344+
if let Some((p_ttx, p_position)) = self.prev_sv_state.get(&c.sv) {
345+
let dt = (t_tx - *p_ttx).to_seconds();
346+
interpolated.velocity =
347+
Some((rot * interpolated.position() - rot * p_position) / dt);
348+
}
349+
350+
self.prev_sv_state
351+
.insert(c.sv, (t_tx, interpolated.position()));
352+
353+
if modeling.relativistic_clock_bias {
354+
/*
355+
* following calculations need inst. velocity
356+
*/
357+
if interpolated.velocity.is_some() {
358+
let _orbit = interpolated.orbit(t_tx, self.earth_frame);
359+
const EARTH_SEMI_MAJOR_AXIS_WGS84: f64 = 6378137.0_f64;
360+
const EARTH_GRAVITATIONAL_CONST: f64 = 3986004.418 * 10.0E8;
361+
let orbit = interpolated.orbit(t_tx, self.earth_frame);
362+
let ea_rad = deg2rad(orbit.ea_deg());
363+
let gm = (EARTH_SEMI_MAJOR_AXIS_WGS84
364+
* EARTH_GRAVITATIONAL_CONST)
365+
.sqrt();
366+
let bias = -2.0_f64 * orbit.ecc() * ea_rad.sin() * gm
367+
/ SPEED_OF_LIGHT
368+
/ SPEED_OF_LIGHT
369+
* Unit::Second;
370+
debug!(
371+
"{:?} ({}) : relativistic clock bias: {}",
372+
t_tx, c.sv, bias
373+
);
374+
c.clock_corr += bias;
375+
}
376+
}
369377

370-
debug!(
371-
"{:?} ({}) : interpolated state: {:?}",
372-
t_tx, c.sv, interpolated.position
373-
);
378+
debug!(
379+
"{:?} ({}) : interpolated state: {:?}",
380+
t_tx, c.sv, interpolated.position
381+
);
374382

375-
c.state = Some(interpolated);
376-
Some(c)
377-
} else {
378-
warn!("{:?} ({}) : interpolation failed", t_tx, c.sv);
379-
None
383+
c.state = Some(interpolated);
384+
Some(c)
385+
} else {
386+
warn!("{:?} ({}) : interpolation failed", t_tx, c.sv);
387+
None
388+
}
389+
},
390+
Err(e) => {
391+
error!("{} - transsmision time error: {:?}", c.sv, e);
392+
None
393+
},
380394
}
381395
})
382396
.collect();
@@ -519,8 +533,11 @@ impl<
519533
self.filter_state.clone(),
520534
)?;
521535

522-
if filter != Filter::None {
523-
self.filter_state = new_state;
536+
let validator = SolutionValidator::new(&self.apriori.ecef, &pool, &w, &pvt_solution);
537+
538+
let valid = validator.valid(solver_opts);
539+
if valid.is_err() {
540+
return Err(Error::InvalidatedSolution(valid.err().unwrap()));
524541
}
525542

526543
if let Some((prev_t, prev_pvt)) = &self.prev_pvt {
@@ -529,16 +546,13 @@ impl<
529546

530547
self.prev_pvt = Some((t, pvt_solution.clone()));
531548

532-
let validator = SolutionValidator::new(&self.apriori.ecef, &pool, &w, &pvt_solution);
533-
534-
let valid = validator.valid(solver_opts);
535-
if valid.is_err() {
536-
return Err(Error::InvalidatedSolution(valid.err().unwrap()));
549+
if filter != Filter::None {
550+
self.filter_state = new_state;
537551
}
538552

539553
/*
540-
* slightly rework the solution so it ""physically"" (/ looks like)
541-
* what we expect based on the predefined setup.
554+
* slightly rework the solution so it ""looks"" like
555+
* what we expect based on the defined setup.
542556
*/
543557
if let Some(alt) = self.cfg.fixed_altitude {
544558
pvt_solution.pos.z = self.apriori.ecef.z - alt;

0 commit comments

Comments
 (0)