Skip to content

Commit

Permalink
working on crate upgrade (#6)
Browse files Browse the repository at this point in the history 
* working on crate upgrade

---------

Signed-off-by: Guillaume W. Bres <[email protected]>
  • Loading branch information
@gwbres
gwbres authored Apr 13, 2024
1 parent cb00dbd commit f1ca493
Show file tree
Hide file tree
Showing 5 changed files with 30 additions and 141 deletions.
43 changes: 7 additions & 36 deletions src/candidate.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,9 @@ use itertools::Itertools;
use log::debug;

use nyx::cosmic::SPEED_OF_LIGHT;
use nyx::linalg::{DVector, Matrix3, MatrixXx4};
use nyx::linalg::{DVector, MatrixXx4};

use crate::prelude::{Config, Duration, Epoch, InterpolatedPosition, InterpolationResult, Vector3};
use crate::prelude::{Config, Duration, Epoch, InterpolationResult, Vector3};
use crate::solutions::{PVTBias, PVTSVData};
use crate::{
bias,
Expand Down Expand Up @@ -68,6 +68,10 @@ impl Candidate {
phase: Vec<Observation>,
doppler: Vec<Observation>,
) -> Result<Self, Error> {
debug!(
"{:?} ({}) - clock state: {:?} - correction {}",
t, sv, clock_state, clock_corr,
);
if code.is_empty() {
Err(Error::NeedsAtLeastOnePseudoRange)
} else {
Expand Down Expand Up @@ -250,7 +254,6 @@ impl Candidate {
row_index: usize,
y: &mut DVector<f64>,
g: &mut MatrixXx4<f64>,
r_sun: &Vector3<f64>,
) -> Result<PVTSVData, Error> {
// state
let state = self.state.ok_or(Error::UnresolvedState)?;
Expand All @@ -270,39 +273,7 @@ impl Candidate {
};

let (x0, y0, z0) = apriori;

/*
* Compensate for APC (if desired)
*/
let sv_p = match cfg.modeling.sv_apc {
true => {
match state.position {
InterpolatedPosition::MassCenter(r_sat) => {
let delta_apc = 0.0_f64; //TODO
let k = -r_sat
/ (r_sat[0].powi(2) + r_sat[1].powi(2) + r_sat[3].powi(2)).sqrt();
let norm = ((r_sun[0] - r_sat[0]).powi(2)
+ (r_sun[1] - r_sat[1]).powi(2)
+ (r_sun[2] - r_sat[2]).powi(2))
.sqrt();

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_dot = Vector3::<f64>::new(
(i[0] + j[0] + k[0]) * delta_apc,
(i[1] + j[1] + k[1]) * delta_apc,
(i[2] + j[2] + k[2]) * delta_apc,
);
r_sat + r_dot
},
InterpolatedPosition::AntennaPhaseCenter(r_sat) => r_sat,
}
},
false => state.position(),
};

let (sv_x, sv_y, sv_z) = (sv_p[0], sv_p[1], sv_p[2]);
let (sv_x, sv_y, sv_z) = (state.position[0], state.position[1], state.position[2]);

let mut sv_data = PVTSVData::default();
sv_data.azimuth = azimuth;
Expand Down
9 changes: 1 addition & 8 deletions src/cfg.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,10 +132,6 @@ fn default_relativistic_path_range() -> bool {
false
}

fn default_sv_apc() -> bool {
false
}

fn default_weight_matrix() -> Option<WeightMatrix> {
None
//Some(WeightMatrix::MappingFunction(
Expand Down Expand Up @@ -233,8 +229,6 @@ pub struct Modeling {
#[cfg_attr(feature = "serde", serde(default))]
pub sv_total_group_delay: bool,
#[cfg_attr(feature = "serde", serde(default))]
pub sv_apc: bool,
#[cfg_attr(feature = "serde", serde(default))]
pub relativistic_clock_bias: bool,
#[cfg_attr(feature = "serde", serde(default))]
pub relativistic_path_range: bool,
Expand All @@ -250,7 +244,6 @@ impl Default for Modeling {
fn default() -> Self {
Self {
sv_clock_bias: default_sv_clock(),
sv_apc: default_sv_apc(),
iono_delay: default_iono(),
tropo_delay: default_tropo(),
sv_total_group_delay: default_sv_tgd(),
Expand Down Expand Up @@ -279,7 +272,7 @@ pub struct Config {
/// (Position) interpolation filter order.
/// A minimal order must be respected for correct results.
/// - 7 is the minimal value for metric resolution
/// - 11 is the standard when aiming at submetric resolution
/// - 11 is the standard when pushing for submetric resolution
#[cfg_attr(feature = "serde", serde(default = "default_interp"))]
pub interp_order: usize,
/// Fixed altitude : reduces the need of 4 to 3 SV to resolve a solution
Expand Down
3 changes: 2 additions & 1 deletion src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,10 @@ pub mod prelude {
pub use crate::candidate::{Candidate, Observation};
pub use crate::cfg::{Config, Filter, Method};
pub use crate::solutions::{PVTSolution, PVTSolutionType};
pub use crate::solver::{InterpolatedPosition, InterpolationResult, Solver};
pub use crate::solver::{InterpolationResult, Solver};
// re-export
pub use gnss::prelude::{Constellation, SV};
pub use hifitime::{Duration, Epoch, TimeScale};
pub use nalgebra::Vector3;
pub use nyx::md::prelude::{Arc, Bodies, Cosm, Frame, LightTimeCalc};
}
2 changes: 1 addition & 1 deletion src/solutions/validator.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ impl SolutionValidator {
.unwrap();

let pr = cd.prefered_pseudorange().unwrap().value;
let state = cd.state.unwrap().position();
let state = cd.state.unwrap().position;
let estimate = apriori_ecef + solution.pos;

let (sv_x, sv_y, sv_z) = (state[0], state[1], state[2]);
Expand Down
114 changes: 19 additions & 95 deletions src/solver.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,7 @@ use thiserror::Error;
use nyx::cosmic::eclipse::{eclipse_state, EclipseState};
use nyx::cosmic::Orbit;
use nyx::cosmic::SPEED_OF_LIGHT;
use nyx::md::prelude::{Arc, Cosm};
use nyx::md::prelude::{Bodies, Frame, LightTimeCalc};
use nyx::md::prelude::{Arc, Cosm, Frame};

use gnss::prelude::SV;

Expand Down Expand Up @@ -62,15 +61,6 @@ pub enum Error {
UninitializedKalmanFilter,
}

/// Position interpolator helper
#[derive(Copy, Clone, Debug, PartialEq)]
pub enum InterpolatedPosition {
/// Providing Mass Center position
MassCenter(Vector3<f64>),
/// Providing APC position
AntennaPhaseCenter(Vector3<f64>),
}

#[derive(Debug, Clone)]
pub(crate) struct LSQState {
/* p matrix */
Expand Down Expand Up @@ -112,40 +102,26 @@ pub(crate) enum FilterState {
KfState(KfState),
}

impl Default for InterpolatedPosition {
fn default() -> Self {
Self::AntennaPhaseCenter(Vector3::<f64>::default())
}
}

/// Interpolation result (state vector) that needs to be
/// resolved for every single candidate.
#[derive(Copy, Clone, Debug, Default, PartialEq)]
pub struct InterpolationResult {
/// Position vector in [m] ECEF
pub position: InterpolatedPosition,
/// Elevation compared to reference position and horizon in [°]
pub elevation: f64,
/// Azimuth compared to reference position and magnetic North in [°]
pub azimuth: f64,
/// APC Position vector in [m] ECEF
pub position: Vector3<f64>,
// Velocity vector in [m/s] ECEF that we calculated ourselves
velocity: Option<Vector3<f64>>,
}

impl InterpolationResult {
/// Builds InterpolationResults from a Mass Center (MC) position
/// as ECEF [m] coordinates
pub fn from_mass_center_position(pos: (f64, f64, f64)) -> Self {
let mut s = Self::default();
s.position = InterpolatedPosition::MassCenter(Vector3::<f64>::new(pos.0, pos.1, pos.2));
s
}
/// Builds InterpolationResults from an Antenna Phase Center (APC) position
/// as ECEF [m] coordinates
pub fn from_apc_position(pos: (f64, f64, f64)) -> Self {
let mut s = Self::default();
s.position =
InterpolatedPosition::AntennaPhaseCenter(Vector3::<f64>::new(pos.0, pos.1, pos.2));
s.position = Vector3::<f64>::new(pos.0, pos.1, pos.2);
s
}
/// Builds Self with given SV (elevation, azimuth) attitude
Expand All @@ -155,17 +131,11 @@ impl InterpolationResult {
s.azimuth = elev_azim.1;
s
}
pub(crate) fn position(&self) -> Vector3<f64> {
match self.position {
InterpolatedPosition::MassCenter(mc) => mc,
InterpolatedPosition::AntennaPhaseCenter(pc) => pc,
}
}
pub(crate) fn velocity(&self) -> Option<Vector3<f64>> {
self.velocity
}
pub(crate) fn orbit(&self, dt: Epoch, frame: Frame) -> Orbit {
let p = self.position();
let p = self.position;
let v = self.velocity().unwrap_or_default();
Orbit::cartesian(
p[0] / 1000.0,
Expand All @@ -180,30 +150,21 @@ impl InterpolationResult {
}
}

/// PVT Solver
/// PVT Solver.
/// I: Interpolated SV APC coordinates interface.
/// You are required to provide APC coordinates at requested ("t", "sv"),
/// expressed in meters [ECEF], for this to proceed.
#[derive(Debug, Clone)]
pub struct Solver<APC, I>
pub struct Solver<I>
where
APC: Fn(Epoch, SV, f64) -> Option<(f64, f64, f64)>,
I: Fn(Epoch, SV, usize) -> Option<InterpolationResult>,
{
/// Solver parametrization
pub cfg: Config,
/// apriori position
pub apriori: AprioriPosition,
/// SV state interpolation method. It is mandatory
/// to resolve the SV state at the requested Epoch otherwise the solver
/// will not proceed further. User should provide the interpolation method.
/// Other parameters are SV: Space Vehicle identity we want to resolve, and "usize" interpolation order.
/// Interpolated SV state.
pub interpolator: I,
/// If the Position Interpolator I returns Mass Center positions,
/// and [Config].modeling.sv_apc is turned on, we need to apply the
/// tiny correction to convert the MC to APC.
/// This method should return for a given SV and frequency at current Epoch,
/// the correction expressed as ENU offset in meters.
/// If the interpolator returns Antenna Phase Centers directly, or
/// the SV APC correction is turned off, this interface remains completely idle.
pub apc_correction: APC,
/* Cosmic model */
cosmic: Arc<Cosm>,
/*
Expand All @@ -224,45 +185,30 @@ where
filter_state: Option<FilterState>,
/* prev. state vector for internal velocity determination */
prev_sv_state: HashMap<SV, (Epoch, Vector3<f64>)>,
/* already determined APC retrieve */
sv_apc_corrections: Vec<((SV, f64), (f64, f64, f64))>,
}

impl<
APC: std::ops::Fn(Epoch, SV, f64) -> Option<(f64, f64, f64)>,
I: std::ops::Fn(Epoch, SV, usize) -> Option<InterpolationResult>,
> Solver<APC, I>
{
pub fn new(
cfg: &Config,
apriori: AprioriPosition,
interpolator: I,
apc_correction: APC,
) -> Result<Self, Error> {
impl<I: std::ops::Fn(Epoch, SV, usize) -> Option<InterpolationResult>> Solver<I> {
pub fn new(cfg: &Config, apriori: AprioriPosition, interpolator: I) -> Result<Self, Error> {
let cosmic = Cosm::de438();
let sun_frame = cosmic.frame("Sun J2000");
let earth_frame = cosmic.frame("EME2000");

/*
* print some infos on latched config
*/
if cfg.method == Method::SPP && cfg.min_sv_sunlight_rate.is_some() {
warn!("eclipse filter is not meaningful when using spp strategy");
warn!("Eclipse filter is not meaningful in SPP mode");
}
if cfg.modeling.relativistic_path_range {
warn!("relativistic path range cannot be modeled at the moment");
warn!("Relativistic path range cannot be modeled at the moment");
}

Ok(Self {
cosmic,
sun_frame,
earth_frame,
apriori,
interpolator,
apc_correction,
cfg: cfg.clone(),
prev_sv_state: HashMap::new(),
sv_apc_corrections: Vec::new(),
filter_state: Option::<FilterState>::None,
prev_pvt: Option::<(Epoch, PVTSolution)>::None,
})
Expand Down Expand Up @@ -336,19 +282,17 @@ impl<
),
};

interpolated.position = InterpolatedPosition::AntennaPhaseCenter(
rot * interpolated.position(),
);
interpolated.position = 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);
Some((rot * interpolated.position - rot * p_position) / dt);
}

self.prev_sv_state
.insert(c.sv, (t_tx, interpolated.position()));
.insert(c.sv, (t_tx, interpolated.position));

if modeling.relativistic_clock_bias {
/*
Expand All @@ -374,11 +318,7 @@ impl<
c.clock_corr += bias;
}
}

debug!(
"{:?} ({}) : interpolated state: {:?}",
t_tx, c.sv, interpolated.position
);
debug!("{:?} ({}) : {:?}", t_tx, c.sv, interpolated);

c.state = Some(interpolated);
Some(c)
Expand Down Expand Up @@ -497,8 +437,6 @@ impl<
let mut g = MatrixXx4::<f64>::zeros(nb_candidates);
let mut pvt_sv_data = HashMap::<SV, PVTSVData>::with_capacity(nb_candidates);

let r_sun = Self::sun_unit_vector(&self.earth_frame, &self.cosmic, t);

for (row_index, cd) in pool.iter().enumerate() {
if let Ok(sv_data) = cd.resolve(
t,
Expand All @@ -510,7 +448,6 @@ impl<
row_index,
&mut y,
&mut g,
&r_sun,
) {
pvt_sv_data.insert(cd.sv, sv_data);
}
Expand Down Expand Up @@ -569,19 +506,6 @@ impl<

Ok((t, pvt_solution))
}
/*
* Evaluates Sun/Earth vector in meter ECEF at given Epoch
*/
fn sun_unit_vector(ref_frame: &Frame, cosmic: &Arc<Cosm>, t: Epoch) -> Vector3<f64> {
let sun_body = Bodies::Sun;
let orbit =
cosmic.celestial_state(sun_body.ephem_path(), t, *ref_frame, LightTimeCalc::None);
Vector3::new(
orbit.x_km * 1000.0,
orbit.y_km * 1000.0,
orbit.z_km * 1000.0,
)
}
/*
* Returns nb of vehicles we need to gather
*/
Expand Down

0 comments on commit f1ca493

Please sign in to comment.