Skip to content

Commit

Permalink
working on rtk module
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 03af6f9 commit 4f93a0d
Show file tree
Hide file tree
Showing 3 changed files with 157 additions and 134 deletions.
8 changes: 5 additions & 3 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::{DVector, Matrix4xX, Vector4};
use nalgebra::base::{DVector, MatrixXx4, Vector4};
/*
* Solver solution estimate
* is always expressed as a correction of an 'a priori' position
Expand All @@ -25,10 +25,12 @@ impl SolverEstimate {
* Builds a new SolverEstimate from `g` Nav Matrix,
* and `y` Nav Vector
*/
pub fn new(g: Matrix4xX<f64>, y: DVector<f64>) -> Option<Self> {
pub fn new(g: MatrixXx4<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;
let x = g.pseudo_inverse(1.0E-6).unwrap() * y;
//let x = g_prime.clone() * y;
//let x = q.clone() * x;
let pdop = (q[(1, 1)] + q[(2, 2)] + q[(3, 3)]).sqrt();
let tdop = q[(4, 4)].sqrt();
Some(Self {
Expand Down
273 changes: 148 additions & 125 deletions gnss-rtk/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,8 @@ use hifitime::Unit;

extern crate nyx_space as nyx;

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

mod estimate;
Expand All @@ -36,9 +37,9 @@ use thiserror::Error;

#[derive(Debug, Clone, Error)]
pub enum SolverError {
#[error("provided context is either unsufficient or invalid for any position solving")]
#[error("provided context is either not sufficient or invalid")]
Unfeasible,
#[error("apriori position is not defined - initialization it not complete!")]
#[error("apriori position is not defined")]
UndefinedAprioriPosition,
#[error("failed to initialize solver - \"{0}\"")]
InitializationError(String),
Expand Down Expand Up @@ -127,31 +128,20 @@ impl Solver {
// total_dropped,
// total
// );

// 2: interpolate: if need be
//if !ctx.interpolated {
// trace!("orbit interpolation..");
// let order = self.opts.interp_order;
// ctx.orbit_interpolation(order, None);
// //TODO could be nice to have some kind of timing/perf evaluation here
// // and also total number of required interpolations
//}

//initialization
self.nth_epoch = 0;
/*
* Solving needs a ref. position
*/
if self.opts.rcvr_position.is_none() {
// defined in context ?
let position = ctx
.ground_position()
.ok_or(SolverError::InitializationError(String::from(
"Missing ref. position",
)))?;
self.opts.rcvr_position = Some(position);
let position = ctx.ground_position();
if let Some(position) = position {
self.opts.rcvr_position = Some(position);
} else {
return Err(SolverError::UndefinedAprioriPosition);
}
}

self.nth_epoch = 0;
self.initiated = true;
Ok(())
}
Expand All @@ -169,7 +159,7 @@ impl Solver {

let modeling = self.opts.modeling;

// grab work instant
// 0: grab work instant
let t = ctx.primary_data().epoch().nth(self.nth_epoch);

if t.is_none() {
Expand All @@ -180,7 +170,7 @@ impl Solver {
let t = t.unwrap();
let interp_order = self.opts.interp_order;

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

/* 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() {
if let Some((x_km, y_km, z_km)) = sp3.sv_position_interpolate(*sv, t, interp_order)
{
sv_pos.insert(*sv, (x_km, y_km, z_km));
} else if let Some(nav) = ctx.navigation_data() {
if let Some((x_km, y_km, z_km)) =
nav.sv_position_interpolate(*sv, t, interp_order)
{
sv_pos.insert(*sv, (x_km, y_km, z_km));
}
}
} else {
if let Some(nav) = ctx.navigation_data() {
if let Some((x_km, y_km, z_km)) =
nav.sv_position_interpolate(*sv, t, interp_order)
{
sv_pos.insert(*sv, (x_km, y_km, z_km));
}
}
}
}
/* remove sv in eclipse */
if self.solver == SolverType::PPP {
if let Some(min_rate) = self.opts.min_sv_sunlight_rate {
sv_pos.retain(|sv, (x_km, y_km, z_km)| {
let state = self.eclipse_state(*x_km, *y_km, *z_km, t);
let eclipsed = match state {
EclipseState::Umbra => true,
EclipseState::Visibilis => false,
EclipseState::Penumbra(r) => {
debug!("{} state: {}", sv, state);
r < min_rate
},
};

if eclipsed {
debug!("dropping eclipsed {}", sv);
}
!eclipsed
});
}
}
// 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
// 2: retrieve pseudorange
// TODO: use IONO free combination in PPP
let pr: Vec<_> = ctx
.primary_data()
.pseudo_range()
Expand All @@ -262,29 +200,123 @@ impl Solver {
}
})
.collect();

// 5: position @ tx
let mut sv_pos: HashMap<Sv, Vector3<f64>> = HashMap::new();
for t_tx in t_tx {
if modeling.relativistic_clock_corr {
// Needs sv_spoeed()
// -2 * r.dot(v) / c / c
// 3: t_tx
let mut t_tx: HashMap<Sv, Epoch> = HashMap::new();
for sv in &elected_sv {
// pr for this SV @ this epoch
let pr = pr
.iter()
.filter_map(|(svnn, pr)| if svnn == sv { Some(*pr) } else { None })
.reduce(|pr, _| pr);
// evaluation
if let Some(pr) = pr {
if let Some(sv_t_tx) = Self::sv_transmission_time(ctx, *sv, t, pr, modeling) {
t_tx.insert(*sv, sv_t_tx);
}
} else {
debug!("{}@{}: missing pseudorange", t, sv);
}
if modeling.earth_rotation {
// dt = || rsat - rcvr0 || /c
// rsat = R3 * we * dt * rsat
// we = 7.2921151467 E-5
}
// 4: sv position @ tx
let mut sv_pos: HashMap<Sv, Vector3<f64>> = HashMap::new();
for (sv, t_tx) in &t_tx {
// relativistic clock offset correction
let t_tx = match modeling.relativistic_clock_corr {
true => {
//TODO
let e = 1.204112719279E-2;
let sqrt_a = 5.153704689026E3;
let sqrt_mu = (3986004.418E8_f64).sqrt();
//let dt = -2.0_f64 * sqrt_a * sqrt_mu / SPEED_OF_LIGHT / SPEED_OF_LIGHT * e * elev.sin();
t_tx //TODO
},
false => t_tx,
};
if let Some(sp3) = ctx.sp3_data() {
//TODO : SP3 needs APC correction
// which needs Self::eval_sun_vector3D
if let Some((x_km, y_km, z_km)) =
sp3.sv_position_interpolate(*sv, *t_tx, interp_order)
{
let mut pos = vector![x_km * 1.0E3, y_km * 1.0E3, z_km * 1.0E3];
if modeling.earth_rotation {
//TODO
// dt = || rsat - rcvr0 || /c
// rsat = R3 * we * dt * rsat
// we = 7.2921151467 E-5
}
// Eclipse filter
if self.solver == SolverType::PPP {
if let Some(min_rate) = self.opts.min_sv_sunlight_rate {
let state = self.eclipse_state(x_km, y_km, z_km, *t_tx);
let eclipsed = match state {
EclipseState::Umbra => true,
EclipseState::Visibilis => false,
EclipseState::Penumbra(r) => {
debug!("{} state: {}", sv, state);
r < min_rate
},
};
if eclipsed {
debug!("dropping eclipsed {}", sv);
} else {
sv_pos.insert(*sv, pos);
}
} else {
sv_pos.insert(*sv, pos);
}
} else {
sv_pos.insert(*sv, pos);
}
} else {
debug!("{}@{}: sp3 interpolation failed", t, sv);
}
} else if let Some(nav) = ctx.navigation_data() {
if let Some((x_km, y_km, z_km)) =
nav.sv_position_interpolate(*sv, *t_tx, interp_order)
{
let mut pos = vector![x_km * 1.0E3, y_km * 1.0E3, z_km * 1.0E3];
if modeling.earth_rotation {
//TODO
// dt = || rsat - rcvr0 || /c
// rsat = R3 * we * dt * rsat
// we = 7.2921151467 E-5
}
// Eclipse filter
if self.solver == SolverType::PPP {
if let Some(min_rate) = self.opts.min_sv_sunlight_rate {
let state = self.eclipse_state(x_km, y_km, z_km, *t_tx);
let eclipsed = match state {
EclipseState::Umbra => true,
EclipseState::Visibilis => false,
EclipseState::Penumbra(r) => {
debug!("{} state: {}", sv, state);
r < min_rate
},
};
if eclipsed {
debug!("dropping eclipsed {}", sv);
} else {
sv_pos.insert(*sv, pos);
}
} else {
sv_pos.insert(*sv, pos);
}
} else {
sv_pos.insert(*sv, pos);
}
} else {
debug!("{}@{}: kepler interpolation failed", t, sv);
}
}
}

// 6: form matrix
let mut y = DVector::<f64>::zeros(elected_sv.len());
let mut g = Matrix4xX::<f64>::zeros(elected_sv.len());
let mut g = MatrixXx4::<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()
Expand Down Expand Up @@ -316,7 +348,9 @@ impl Solver {
}

// 7: resolve
//trace!("y: {} | g: {}", y, g);
let estimate = SolverEstimate::new(g, y);

if estimate.is_none() {
self.nth_epoch += 1;
return Err(SolverError::SolvingError(t));
Expand All @@ -327,45 +361,34 @@ impl Solver {
/*
* Evalutes T_tx transmission time, for given Sv at desired 't'
*/
fn sv_transmission_time(ctx: &QcContext, sv: Sv, t: Epoch, m: Modeling) -> Option<Epoch> {
fn sv_transmission_time(
ctx: &QcContext,
sv: Sv,
t: Epoch,
pr: f64,
m: Modeling,
) -> Option<Epoch> {
let nav = ctx.navigation_data()?;
// need one pseudo range observation for this SV @ 't'
let mut pr = ctx
.primary_data()
.pseudo_range()
.filter_map(|((e, flag), svnn, _, p)| {
if e == t && flag.is_ok() && svnn == sv {
Some(p)
} else {
None
}
})
.take(1);
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()?);

if m.sv_clock_bias {
let dt_sat = nav.sv_clock_bias(sv, e_tx)?;
debug!("{}@{} | dt_sat {}", sv, t, dt_sat);
e_tx -= dt_sat;
}
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()?);

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

if m.sv_total_group_delay {
if let Some(nav) = ctx.navigation_data() {
if let Some(tgd) = nav.sv_tgd(sv, t) {
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");
None
}
debug!("{}@{} | t_tx {}", sv, t, e_tx);
Some(e_tx)
}
/*
* Evaluates Sun/Earth vector, <!> expressed in Km <!>
Expand Down
10 changes: 4 additions & 6 deletions rinex/src/navigation/ephemeris.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -392,12 +392,10 @@ impl Ephemeris {

Some((x_k / 1000.0, y_k / 1000.0, z_k / 1000.0))
}
/*
* Returns Sv position in km ECEF, based off Self Ephemeris data,
* and for given Satellite Vehicle at given Epoch.
* Either by solving Kepler equations, or directly if such data is available.
*/
pub(crate) fn sv_position(&self, sv: &Sv, epoch: Epoch) -> Option<(f64, f64, f64)> {
/// Returns Sv position in km ECEF, based off Self Ephemeris data,
/// and for given Satellite Vehicle at given Epoch.
/// Either by solving Kepler equations, or directly if such data is available.
pub fn sv_position(&self, sv: &Sv, epoch: Epoch) -> Option<(f64, f64, f64)> {
let (x_km, y_km, z_km) = (
self.get_orbit_f64("satPosX"),
self.get_orbit_f64("satPosY"),
Expand Down

0 comments on commit 4f93a0d

Please sign in to comment.