Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix nav/doris feature definition #256

Merged
merged 1 commit into from
Jul 23, 2024
Merged

Fix nav/doris feature definition #256

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion rinex/src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,6 @@ pub mod prelude {
pub use crate::context::{ProductType, RnxContext};
pub use crate::cospar::COSPAR;
pub use crate::domes::Domes;
#[cfg(feature = "doris")]
pub use crate::doris::Station;
pub use crate::ground_position::GroundPosition;
pub use crate::header::Header;
Expand Down
258 changes: 127 additions & 131 deletions rinex/src/navigation/ephemeris.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -241,128 +241,6 @@ impl Ephemeris {
pub(crate) fn a_dot(&self) -> Option<f64> {
self.get_orbit_f64("a_dot")
}
/*
* Get the difference between toe and observation epoch,
* as total seconds elapsed in GPS timescale
*/
pub(crate) fn tk(&self, sv: SV, t: Epoch) -> Option<f64> {
let toe = self.toe_gpst(sv.timescale()?)?;
let t_dur = t.to_gpst_duration();
let t_k = (t_dur - toe.duration).to_seconds();
let dur = Self::max_dtoe(sv.constellation)?;
if t_k.abs() <= dur.to_seconds() {
Some(t_k)
} else {
None
}
}
/*
* get ephemerisHelper
*/
pub(crate) fn ephemeris_helper(&self, sv: SV, t: Epoch) -> Option<EphemerisHelper> {
// const
let gm_m3_s2 = Constants::gm(sv);
let omega = Constants::omega(sv);
let dtr_f = Constants::dtr_f(sv);
let mut helper = EphemerisHelper {
sv,
..Default::default()
};
// set t_k
if let Some(t_k) = self.tk(sv, t) {
helper.t_k = t_k
} else {
log::trace!("{} ephemeris missing or invalid at {}", sv, t);
return None;
}
let mut kepler = self.kepler()?;
// considering the filed a_dot
if let Some(a_dot) = self.a_dot() {
kepler.a += a_dot * helper.t_k;
}
let perturbations = self.perturbations()?;

// m_k, e_k, v_k calculation
let n0 = (gm_m3_s2 / kepler.a.powi(3)).sqrt();
let n = n0 + perturbations.dn;
let m_k = kepler.m_0 + n * helper.t_k;
// Iterative calculation of e_k
let mut e_k_lst: f64 = 0.0;
let mut e_k: f64 = 0.0;
let mut i = 0;
loop {
e_k = m_k + kepler.e * e_k_lst.sin();
if (e_k - e_k_lst).abs() < 1e-10 {
break;
}
i += 1;
e_k_lst = e_k;
}
if i >= constants::MaxIterNumber::KEPLER {
log::warn!("{} kepler iteration overflow", sv);
}
let (sin_e_k, cos_e_k) = e_k.sin_cos();
let v_k = ((1.0 - kepler.e.powi(2)).sqrt() * sin_e_k).atan2(cos_e_k - kepler.e);

// u_k, r_k, i_k
let phi_k = v_k + kepler.omega;
let (sin2_phi_k, cos2_phi_k) = (2.0 * phi_k).sin_cos();
let det_u_k = perturbations.cus * sin2_phi_k + perturbations.cuc * cos2_phi_k;
let det_r_k = perturbations.crs * sin2_phi_k + perturbations.crc * cos2_phi_k;
let det_i_k = perturbations.cis * sin2_phi_k + perturbations.cic * cos2_phi_k;
helper.u_k = phi_k + det_u_k;
helper.r_k = kepler.a * (1.0 - kepler.e * e_k.cos()) + det_r_k;
helper.i_k = kepler.i_0 + perturbations.i_dot * helper.t_k + det_i_k;

// omega_k
// MEO (GPS, Galieo, BeiDou)
// IGSO(BeiDou)
if Constants::is_beidou_geo(sv) {
helper.omega_k =
kepler.omega_0 + perturbations.omega_dot * helper.t_k - omega * kepler.toe;
} else {
helper.omega_k = kepler.omega_0 + (perturbations.omega_dot - omega) * helper.t_k
- omega * kepler.toe;
}

// calculate First Derivative of e_k,phi_k,u_k,r_k,i_k,omega_k
let fd_e_k = n / (1.0 - kepler.e * e_k.cos());
let fd_phi_k = ((1.0 + kepler.e) / (1.0 - kepler.e)).sqrt()
* ((v_k / 2.0).cos() / (e_k / 2.0).cos()).powi(2)
* fd_e_k;
helper.fd_u_k =
(perturbations.cus * cos2_phi_k - perturbations.cuc * sin2_phi_k) * fd_phi_k * 2.0
+ fd_phi_k;
helper.fd_r_k = kepler.a * kepler.e * e_k.sin() * fd_e_k
+ 2.0 * (perturbations.crs * cos2_phi_k - perturbations.crc * sin2_phi_k) * fd_phi_k;
helper.fd_i_k = perturbations.i_dot
+ 2.0 * (perturbations.cis * cos2_phi_k - perturbations.cic * sin2_phi_k) * fd_phi_k;
helper.fd_omega_k = perturbations.omega_dot - omega;

// orbit position
helper.orbit_position = (helper.r_k * helper.u_k.cos(), helper.r_k * helper.u_k.sin());

// Relativistic Effect Correction
helper.dtr = dtr_f * kepler.e * kepler.a.sqrt() * e_k.sin();
helper.fd_dtr = dtr_f * kepler.e * kepler.a.sqrt() * e_k.cos() * fd_e_k;

// Finally, build the orbit state
helper.orbit = Some(
Orbit::try_keplerian(
kepler.a,
kepler.e,
helper.i_k,
helper.omega_k,
helper.u_k,
v_k,
t,
EARTH_J2000.with_mu_km3_s2(gm_m3_s2 * 1e-9),
)
.unwrap(),
);

Some(helper)
}
/*
* Parses ephemeris from given line iterator
*/
Expand Down Expand Up @@ -527,7 +405,126 @@ impl Ephemeris {
s.set_orbit_f64("omegaDot", perturbations.omega_dot);
s
}
/// Converts Self to [EphemerisHelper]
fn ephemeris_helper(&self, sv: SV, t: Epoch) -> Option<EphemerisHelper> {
// const
let gm_m3_s2 = Constants::gm(sv);
let omega = Constants::omega(sv);
let dtr_f = Constants::dtr_f(sv);
let mut helper = EphemerisHelper {
sv,
..Default::default()
};
// set t_k
if let Some(t_k) = self.tk(sv, t) {
helper.t_k = t_k
} else {
log::trace!("{} ephemeris missing or invalid at {}", sv, t);
return None;
}
let mut kepler = self.kepler()?;
// considering the filed a_dot
if let Some(a_dot) = self.a_dot() {
kepler.a += a_dot * helper.t_k;
}
let perturbations = self.perturbations()?;

// m_k, e_k, v_k calculation
let n0 = (gm_m3_s2 / kepler.a.powi(3)).sqrt();
let n = n0 + perturbations.dn;
let m_k = kepler.m_0 + n * helper.t_k;
// Iterative calculation of e_k
let mut e_k_lst: f64 = 0.0;
let mut e_k: f64 = 0.0;
let mut i = 0;
loop {
e_k = m_k + kepler.e * e_k_lst.sin();
if (e_k - e_k_lst).abs() < 1e-10 {
break;
}
i += 1;
e_k_lst = e_k;
}
if i >= constants::MaxIterNumber::KEPLER {
log::warn!("{} kepler iteration overflow", sv);
}
let (sin_e_k, cos_e_k) = e_k.sin_cos();
let v_k = ((1.0 - kepler.e.powi(2)).sqrt() * sin_e_k).atan2(cos_e_k - kepler.e);

// u_k, r_k, i_k
let phi_k = v_k + kepler.omega;
let (sin2_phi_k, cos2_phi_k) = (2.0 * phi_k).sin_cos();
let det_u_k = perturbations.cus * sin2_phi_k + perturbations.cuc * cos2_phi_k;
let det_r_k = perturbations.crs * sin2_phi_k + perturbations.crc * cos2_phi_k;
let det_i_k = perturbations.cis * sin2_phi_k + perturbations.cic * cos2_phi_k;
helper.u_k = phi_k + det_u_k;
helper.r_k = kepler.a * (1.0 - kepler.e * e_k.cos()) + det_r_k;
helper.i_k = kepler.i_0 + perturbations.i_dot * helper.t_k + det_i_k;

// omega_k
// MEO (GPS, Galieo, BeiDou)
// IGSO(BeiDou)
if Constants::is_beidou_geo(sv) {
helper.omega_k =
kepler.omega_0 + perturbations.omega_dot * helper.t_k - omega * kepler.toe;
} else {
helper.omega_k = kepler.omega_0 + (perturbations.omega_dot - omega) * helper.t_k
- omega * kepler.toe;
}

// calculate First Derivative of e_k,phi_k,u_k,r_k,i_k,omega_k
let fd_e_k = n / (1.0 - kepler.e * e_k.cos());
let fd_phi_k = ((1.0 + kepler.e) / (1.0 - kepler.e)).sqrt()
* ((v_k / 2.0).cos() / (e_k / 2.0).cos()).powi(2)
* fd_e_k;
helper.fd_u_k =
(perturbations.cus * cos2_phi_k - perturbations.cuc * sin2_phi_k) * fd_phi_k * 2.0
+ fd_phi_k;
helper.fd_r_k = kepler.a * kepler.e * e_k.sin() * fd_e_k
+ 2.0 * (perturbations.crs * cos2_phi_k - perturbations.crc * sin2_phi_k) * fd_phi_k;
helper.fd_i_k = perturbations.i_dot
+ 2.0 * (perturbations.cis * cos2_phi_k - perturbations.cic * sin2_phi_k) * fd_phi_k;
helper.fd_omega_k = perturbations.omega_dot - omega;

// orbit position
helper.orbit_position = (helper.r_k * helper.u_k.cos(), helper.r_k * helper.u_k.sin());

// Relativistic Effect Correction
helper.dtr = dtr_f * kepler.e * kepler.a.sqrt() * e_k.sin();
helper.fd_dtr = dtr_f * kepler.e * kepler.a.sqrt() * e_k.cos() * fd_e_k;

// Finally, build the orbit state
helper.orbit = Some(
Orbit::try_keplerian(
kepler.a,
kepler.e,
helper.i_k,
helper.omega_k,
helper.u_k,
v_k,
t,
EARTH_J2000.with_mu_km3_s2(gm_m3_s2 * 1e-9),
)
.unwrap(),
);

Some(helper)
}
/*
* Get the difference between toe and observation epoch,
* as total seconds elapsed in GPS timescale
*/
fn tk(&self, sv: SV, t: Epoch) -> Option<f64> {
let toe = self.toe_gpst(sv.timescale()?)?;
let t_dur = t.to_gpst_duration();
let t_k = (t_dur - toe.duration).to_seconds();
let dur = Self::max_dtoe(sv.constellation)?;
if t_k.abs() <= dur.to_seconds() {
Some(t_k)
} else {
None
}
}
/// Kepler position solver at desired instant "t" for given "sv"
/// based off Self. Self must be correctly selected in navigation
/// record.
Expand All @@ -541,15 +538,14 @@ impl Ephemeris {
let pos = helper.orbit?.radius_km;
Some((pos.x / 1000.0, pos.y / 1000.0, pos.z / 1000.0))
}
/*
* Kepler position and velocity solver at desired instant "t" for given "sv"
* based off Self. Self must be correctly selected in navigation
* record.
* "t" should not be expressed in UTC time scale as the hifitime doesn't consider
* the leap seconds
* See [Bibliography::AsceAppendix3], [Bibliography::JLe19] and [Bibliography::BeiDouICD]
*/
pub(crate) fn kepler2position_velocity(

/// Kepler position and velocity solver at desired instant "t" for given "sv"
/// based off Self. Self must be correctly selected in navigation
/// record.
/// "t" should not be expressed in UTC time scale as the hifitime doesn't consider
/// the leap seconds
/// See [Bibliography::AsceAppendix3], [Bibliography::JLe19] and [Bibliography::BeiDouICD]
fn kepler2position_velocity(
&self,
sv: SV,
t: Epoch,
Expand Down
Loading