From 9998c47f17c02f0e257a20907871395d3b4b46c0 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Sat, 25 May 2024 10:58:44 -0700 Subject: [PATCH 01/11] Add math operator for ECEF --- swiftnav/src/coords.rs | 144 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 144 insertions(+) diff --git a/swiftnav/src/coords.rs b/swiftnav/src/coords.rs index d3c11ec..689429c 100644 --- a/swiftnav/src/coords.rs +++ b/swiftnav/src/coords.rs @@ -47,6 +47,8 @@ //! * "Transformation from Cartesian to Geodetic Coordinates Accelerated by //! Halley’s Method", T. Fukushima (2006), Journal of Geodesy. +use std::ops::{Add, AddAssign, Mul, MulAssign, Sub, SubAssign}; + /// WGS84 geodetic coordinates (Latitude, Longitude, Height) /// /// Internally stored as an array of 3 [f64](std::f64) values: latitude, longitude (both in the given angular units) and height above the geoid in meters @@ -327,6 +329,104 @@ impl AsMut<[f64; 3]> for ECEF { } } +impl Add for ECEF { + type Output = ECEF; + fn add(self, rhs: ECEF) -> ECEF { + ECEF([self.x() + rhs.x(), self.y() + rhs.y(), self.z() + rhs.z()]) + } +} + +impl Add<&ECEF> for ECEF { + type Output = ECEF; + fn add(self, rhs: &ECEF) -> ECEF { + self + *rhs + } +} + +impl Add<&ECEF> for &ECEF { + type Output = ECEF; + fn add(self, rhs: &ECEF) -> ECEF { + *self + *rhs + } +} + +impl AddAssign for ECEF { + fn add_assign(&mut self, rhs: ECEF) { + *self += &rhs; + } +} + +impl AddAssign<&ECEF> for ECEF { + fn add_assign(&mut self, rhs: &ECEF) { + self.0[0] += rhs.x(); + self.0[1] += rhs.y(); + self.0[2] += rhs.z(); + } +} + +impl Sub for ECEF { + type Output = ECEF; + fn sub(self, rhs: ECEF) -> ECEF { + ECEF([self.x() - rhs.x(), self.y() - rhs.y(), self.z() - rhs.z()]) + } +} + +impl Sub<&ECEF> for ECEF { + type Output = ECEF; + fn sub(self, rhs: &ECEF) -> ECEF { + self - *rhs + } +} + +impl Sub<&ECEF> for &ECEF { + type Output = ECEF; + fn sub(self, rhs: &ECEF) -> ECEF { + *self - *rhs + } +} + +impl SubAssign for ECEF { + fn sub_assign(&mut self, rhs: ECEF) { + *self -= &rhs; + } +} + +impl SubAssign<&ECEF> for ECEF { + fn sub_assign(&mut self, rhs: &ECEF) { + self.0[0] -= rhs.x(); + self.0[1] -= rhs.y(); + self.0[2] -= rhs.z(); + } +} + +impl Mul for f64 { + type Output = ECEF; + fn mul(self, rhs: ECEF) -> ECEF { + ECEF([self * rhs.x(), self * rhs.y(), self * rhs.z()]) + } +} + +impl Mul<&ECEF> for f64 { + type Output = ECEF; + fn mul(self, rhs: &ECEF) -> ECEF { + self * *rhs + } +} + +impl MulAssign for ECEF { + fn mul_assign(&mut self, rhs: f64) { + *self *= &rhs; + } +} + +impl MulAssign<&f64> for ECEF { + fn mul_assign(&mut self, rhs: &f64) { + self.0[0] *= *rhs; + self.0[1] *= *rhs; + self.0[2] *= *rhs; + } +} + /// Local North East Down reference frame coordinates /// /// Internally stored as an array of 3 [f64](std::f64) values: N, E, D all in meters @@ -516,4 +616,48 @@ mod tests { assert!(height_err.abs() < MAX_DIST_ERROR_M); } } + + #[test] + fn ecef_ops() { + let a = ECEF::new(1.0, 2.0, 3.0); + let b = ECEF::new(4.0, 5.0, 6.0); + + let result = a + b; + assert_eq!(5.0, result.x()); + assert_eq!(7.0, result.y()); + assert_eq!(9.0, result.z()); + + let result = a + a + a; + assert_eq!(3.0, result.x()); + assert_eq!(6.0, result.y()); + assert_eq!(9.0, result.z()); + + let result = a - b; + assert_eq!(-3.0, result.x()); + assert_eq!(-3.0, result.y()); + assert_eq!(-3.0, result.z()); + + let result = 2.0 * a; + assert_eq!(2.0, result.x()); + assert_eq!(4.0, result.y()); + assert_eq!(6.0, result.z()); + + let mut result = a; + result += b; + assert_eq!(5.0, result.x()); + assert_eq!(7.0, result.y()); + assert_eq!(9.0, result.z()); + + let mut result = a; + result -= b; + assert_eq!(-3.0, result.x()); + assert_eq!(-3.0, result.y()); + assert_eq!(-3.0, result.z()); + + let mut result = a; + result *= 2.0; + assert_eq!(2.0, result.x()); + assert_eq!(4.0, result.y()); + assert_eq!(6.0, result.z()); + } } From f9ee23dcc79b6e912de3cfa1bcf29e2022ceeb55 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Sat, 25 May 2024 11:03:18 -0700 Subject: [PATCH 02/11] Add function to convert GPS time into fractional year representation --- swiftnav/src/time.rs | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/swiftnav/src/time.rs b/swiftnav/src/time.rs index 84b0f7b..7040a21 100644 --- a/swiftnav/src/time.rs +++ b/swiftnav/src/time.rs @@ -312,6 +312,16 @@ impl GpsTime { self.tow().total_cmp(&other) } } + + pub fn to_fractional_year(&self, utc_params: &UtcParams) -> f64 { + let utc = self.to_utc(utc_params); + utc.to_fractional_year() + } + + pub fn to_fractional_year_hardcoded(&self) -> f64 { + let utc = self.to_utc_hardcoded(); + utc.to_fractional_year() + } } impl fmt::Debug for GpsTime { @@ -768,6 +778,21 @@ impl UtcTime { } gps } + + pub fn to_fractional_year(&self) -> f64 { + let year = self.year() as f64; + let days = self.day_of_year() as f64; + let hours = self.hour() as f64; + let minutes = self.minute() as f64; + let seconds = self.seconds(); + let total_days = days + hours / 24. + minutes / 1440. + seconds / 86400.; + + if is_leap_year(self.year()) { + year + total_days / 366.0 + } else { + year + total_days / 365.0 + } + } } impl Default for UtcTime { @@ -868,6 +893,10 @@ impl From for MJD { } } +pub fn is_leap_year(year: u16) -> bool { + ((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0) +} + #[cfg(test)] mod tests { use super::*; @@ -1669,4 +1698,13 @@ mod tests { assert_eq!(gps.wn(), swiftnav_sys::GLO_EPOCH_WN as i16); assert!((gps.tow() - swiftnav_sys::GLO_EPOCH_TOW as f64).abs() < 1e-9); } + + #[test] + fn is_leap_year() { + use super::is_leap_year; + assert!(!is_leap_year(2019)); + assert!(is_leap_year(2020)); + assert!(!is_leap_year(1900)); + assert!(is_leap_year(2000)); + } } From 9477e5d98c9b695043d57644163a33b9f629d800 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Sat, 25 May 2024 11:02:25 -0700 Subject: [PATCH 03/11] Add reference frame transformations --- swiftnav/Cargo.toml | 3 + swiftnav/src/coords.rs | 122 ++++++ swiftnav/src/lib.rs | 1 + swiftnav/src/reference_frame.rs | 656 +++++++++++++++++++++++++++++ swiftnav/tests/reference_frames.rs | 380 +++++++++++++++++ 5 files changed, 1162 insertions(+) create mode 100644 swiftnav/src/reference_frame.rs create mode 100644 swiftnav/tests/reference_frames.rs diff --git a/swiftnav/Cargo.toml b/swiftnav/Cargo.toml index cc7b248..b77216a 100644 --- a/swiftnav/Cargo.toml +++ b/swiftnav/Cargo.toml @@ -13,3 +13,6 @@ rust-version = "1.62.1" rustversion = "1.0" chrono = { version = "0.4", optional = true } swiftnav-sys = { version = "^0.8.1", path = "../swiftnav-sys/" } + +[dev-dependencies] +float_eq = "1.0.1" diff --git a/swiftnav/src/coords.rs b/swiftnav/src/coords.rs index 689429c..81369e3 100644 --- a/swiftnav/src/coords.rs +++ b/swiftnav/src/coords.rs @@ -49,6 +49,11 @@ use std::ops::{Add, AddAssign, Mul, MulAssign, Sub, SubAssign}; +use crate::{ + reference_frame::{get_transformation, ReferenceFrame, TransformationNotFound}, + time::GpsTime, +}; + /// WGS84 geodetic coordinates (Latitude, Longitude, Height) /// /// Internally stored as an array of 3 [f64](std::f64) values: latitude, longitude (both in the given angular units) and height above the geoid in meters @@ -517,8 +522,102 @@ impl Default for AzimuthElevation { } } +/// Complete coordinate used for transforming between reference frames +/// +/// Velocities are optional, but when present they will be transformed +#[derive(Debug, PartialEq, PartialOrd, Clone)] +pub struct Coordinate { + reference_frame: ReferenceFrame, + position: ECEF, + velocity: Option, + epoch: GpsTime, +} + +impl Coordinate { + pub fn new( + reference_frame: ReferenceFrame, + position: ECEF, + velocity: Option, + epoch: GpsTime, + ) -> Self { + Coordinate { + reference_frame, + position, + velocity, + epoch, + } + } + + pub fn without_velocity( + reference_frame: ReferenceFrame, + position: ECEF, + epoch: GpsTime, + ) -> Self { + Coordinate { + reference_frame, + position, + velocity: None, + epoch, + } + } + + pub fn with_velocity( + reference_frame: ReferenceFrame, + position: ECEF, + velocity: ECEF, + epoch: GpsTime, + ) -> Self { + Coordinate { + reference_frame, + position, + velocity: Some(velocity), + epoch, + } + } + + pub fn reference_frame(&self) -> ReferenceFrame { + self.reference_frame + } + + pub fn position(&self) -> ECEF { + self.position + } + + pub fn velocity(&self) -> Option { + self.velocity + } + + pub fn epoch(&self) -> GpsTime { + self.epoch + } + + /// Use the velocity term to adjust the epoch of the coordinate. + /// When a coordinate has no velocity the position won't be changed. + pub fn adjust_epoch(&self, new_epoch: &GpsTime) -> Self { + let dt = + new_epoch.to_fractional_year_hardcoded() - self.epoch.to_fractional_year_hardcoded(); + let v = self.velocity.unwrap_or_default(); + + Coordinate { + position: self.position + dt * v, + velocity: self.velocity, + epoch: *new_epoch, + reference_frame: self.reference_frame, + } + } + + pub fn transform_to(&self, new_frame: ReferenceFrame) -> Result { + let transformation = get_transformation(self.reference_frame, new_frame)?; + Ok(transformation.transform(self)) + } +} + #[cfg(test)] mod tests { + use float_eq::assert_float_eq; + + use crate::time::UtcTime; + use super::*; const D2R: f64 = std::f64::consts::PI / 180.0; @@ -660,4 +759,27 @@ mod tests { assert_eq!(4.0, result.y()); assert_eq!(6.0, result.z()); } + + #[test] + fn coordinate_epoch() { + let initial_epoch = UtcTime::from_date(2020, 1, 1, 0, 0, 0.).to_gps_hardcoded(); + let new_epoch = UtcTime::from_date(2021, 1, 1, 0, 0, 0.).to_gps_hardcoded(); + let initial_coord = Coordinate::with_velocity( + ReferenceFrame::ITRF2020, + ECEF::new(0.0, 0.0, 0.0), + ECEF::new(1.0, 2.0, 3.0), + initial_epoch, + ); + + let new_coord = initial_coord.adjust_epoch(&new_epoch); + + assert_eq!(initial_coord.reference_frame, new_coord.reference_frame); + assert_float_eq!(new_coord.position.x(), 1.0, abs <= 0.001); + assert_float_eq!(new_coord.position.y(), 2.0, abs <= 0.001); + assert_float_eq!(new_coord.position.z(), 3.0, abs <= 0.001); + assert_float_eq!(new_coord.velocity.unwrap().x(), 1.0, abs <= 0.001); + assert_float_eq!(new_coord.velocity.unwrap().y(), 2.0, abs <= 0.001); + assert_float_eq!(new_coord.velocity.unwrap().z(), 3.0, abs <= 0.001); + assert_eq!(new_epoch, new_coord.epoch()); + } } diff --git a/swiftnav/src/lib.rs b/swiftnav/src/lib.rs index 6954276..97e53b6 100644 --- a/swiftnav/src/lib.rs +++ b/swiftnav/src/lib.rs @@ -64,6 +64,7 @@ pub mod ephemeris; pub mod geoid; pub mod ionosphere; pub mod navmeas; +pub mod reference_frame; pub mod signal; pub mod solver; pub mod time; diff --git a/swiftnav/src/reference_frame.rs b/swiftnav/src/reference_frame.rs new file mode 100644 index 0000000..11d9ccc --- /dev/null +++ b/swiftnav/src/reference_frame.rs @@ -0,0 +1,656 @@ +use crate::coords::{Coordinate, ECEF}; +use std::{convert::TryFrom, fmt, str::FromStr}; + +/// Reference Frames +#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)] +pub enum ReferenceFrame { + ITRF2008, + ITRF2014, + ITRF2020, + ETRF2008, + ETRF2014, + ETRF2020, + /// i.e. NAD83(2011) + NAD83_2011, + /// i.e. NAD83(CSRS) - Canadian Spatial Reference System + #[allow(non_camel_case_types)] + NAD83_CSRS, +} + +#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone)] +pub struct InvalidReferenceFrameName(String); + +impl fmt::Display for InvalidReferenceFrameName { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!(f, "Invalid reference frame name ({})", self.0) + } +} + +impl std::error::Error for InvalidReferenceFrameName {} + +impl TryFrom<&str> for ReferenceFrame { + type Error = InvalidReferenceFrameName; + + fn try_from(value: &str) -> Result { + match value { + "ITRF2008" => Ok(ReferenceFrame::ITRF2008), + "ITRF2014" => Ok(ReferenceFrame::ITRF2014), + "ITRF2020" => Ok(ReferenceFrame::ITRF2020), + "ETRF2008" => Ok(ReferenceFrame::ETRF2008), + "ETRF2014" => Ok(ReferenceFrame::ETRF2014), + "ETRF2020" => Ok(ReferenceFrame::ETRF2020), + "NAD83(2011)" => Ok(ReferenceFrame::NAD83_2011), + "NAD83_2011" => Ok(ReferenceFrame::NAD83_2011), + "NAD83(CSRS)" => Ok(ReferenceFrame::NAD83_CSRS), + "NAD83_CSRS" => Ok(ReferenceFrame::NAD83_CSRS), + _ => Err(InvalidReferenceFrameName(value.to_string())), + } + } +} + +impl FromStr for ReferenceFrame { + type Err = InvalidReferenceFrameName; + + fn from_str(s: &str) -> Result { + ReferenceFrame::try_from(s) + } +} + +impl From for &'static str { + fn from(value: ReferenceFrame) -> &'static str { + match value { + ReferenceFrame::ITRF2008 => "ITRF2008", + ReferenceFrame::ITRF2014 => "ITRF2014", + ReferenceFrame::ITRF2020 => "ITRF2020", + ReferenceFrame::ETRF2008 => "ETRF2008", + ReferenceFrame::ETRF2014 => "ETRF2014", + ReferenceFrame::ETRF2020 => "ETRF2020", + ReferenceFrame::NAD83_2011 => "NAD83(2011)", + ReferenceFrame::NAD83_CSRS => "NAD83(CSRS)", + } + } +} + +impl fmt::Display for ReferenceFrame { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!(f, "{}", Into::<&'static str>::into(*self)) + } +} + +/// 15-parameter Helmert transformation parameters +/// +/// This transformation consists of a 3 dimensional translation, +/// 3 dimensional rotation, and a universal scaling. All terms, +/// except for the reference epoch, have a an additional time +/// dependent term. The rotations are typically very small, so +/// the small angle approximation is used. +/// +/// There are several sign and scale conventions in use with +/// Helmert transformations. In this implementation we follow +/// the IERS conventions, meaning the translations are in +/// millimeters, the rotations are in milliarcseconds, and +/// the scaling is in parts per billion. We also follow the +/// IERS convention for the sign of the rotation terms. +#[derive(Debug, PartialEq, PartialOrd, Clone)] +struct TimeDependentHelmertParams { + tx: f64, + tx_dot: f64, + ty: f64, + ty_dot: f64, + tz: f64, + tz_dot: f64, + s: f64, + s_dot: f64, + rx: f64, + rx_dot: f64, + ry: f64, + ry_dot: f64, + rz: f64, + rz_dot: f64, + epoch: f64, +} + +impl TimeDependentHelmertParams { + const TRANSLATE_SCALE: f64 = 1.0e-3; + const SCALE_SCALE: f64 = 1.0e-9; + const ROTATE_SCALE: f64 = (std::f64::consts::PI / 180.0) * (0.001 / 3600.0); + + /// Reverses the transformation. Since this is a linear transformation we simply negate all terms + fn invert(&mut self) { + self.tx *= -1.0; + self.tx_dot *= -1.0; + self.ty *= -1.0; + self.ty_dot *= -1.0; + self.tz *= -1.0; + self.tz_dot *= -1.0; + self.s *= -1.0; + self.s_dot *= -1.0; + self.rx *= -1.0; + self.rx_dot *= -1.0; + self.ry *= -1.0; + self.ry_dot *= -1.0; + self.rz *= -1.0; + self.rz_dot *= -1.0; + } + + /// Apply the transformation on a position at a specific epoch + fn transform_position(&self, position: &ECEF, epoch: f64) -> ECEF { + let dt = epoch - self.epoch; + let tx = (self.tx + self.tx_dot * dt) * Self::TRANSLATE_SCALE; + let ty = (self.ty + self.ty_dot * dt) * Self::TRANSLATE_SCALE; + let tz = (self.tz + self.tz_dot * dt) * Self::TRANSLATE_SCALE; + let s = (self.s + self.s_dot * dt) * Self::SCALE_SCALE; + let rx = (self.rx + self.rx_dot * dt) * Self::ROTATE_SCALE; + let ry = (self.ry + self.ry_dot * dt) * Self::ROTATE_SCALE; + let rz = (self.rz + self.rz_dot * dt) * Self::ROTATE_SCALE; + + let x = position.x() + tx + (s * position.x()) + (-rz * position.y()) + (ry * position.z()); + let y = position.y() + ty + (rz * position.x()) + (s * position.y()) + (-rx * position.z()); + let z = position.z() + tz + (-ry * position.x()) + (rx * position.y()) + (s * position.z()); + + ECEF::new(x, y, z) + } + + /// Apply the transformation on a velocity at a specific position + fn transform_velocity(&self, velocity: &ECEF, position: &ECEF) -> ECEF { + let tx = self.tx_dot * Self::TRANSLATE_SCALE; + let ty = self.ty_dot * Self::TRANSLATE_SCALE; + let tz = self.tz_dot * Self::TRANSLATE_SCALE; + let s = self.s_dot * Self::SCALE_SCALE; + let rx = self.rx_dot * Self::ROTATE_SCALE; + let ry = self.ry_dot * Self::ROTATE_SCALE; + let rz = self.rz_dot * Self::ROTATE_SCALE; + + let x = velocity.x() + tx + (s * position.x()) + (-rz * position.y()) + (ry * position.z()); + let y = velocity.y() + ty + (rz * position.x()) + (s * position.y()) + (-rx * position.z()); + let z = velocity.z() + tz + (-ry * position.x()) + (rx * position.y()) + (s * position.z()); + + ECEF::new(x, y, z) + } +} + +/// Container for a complete transformation which includes +/// both the transformation parameters as well as the source +/// and destination reference frames +#[derive(Debug, PartialEq, PartialOrd, Clone)] +pub struct Transformation { + from: ReferenceFrame, + to: ReferenceFrame, + params: TimeDependentHelmertParams, +} + +impl Transformation { + /// Transform the given coordinate, producing a new coordinate. + /// + /// Reference frame transformations do not change the epoch of the + /// coordinate. + pub fn transform(&self, coord: &Coordinate) -> Coordinate { + assert!( + coord.reference_frame() == self.from, + "Coordinate reference frame does not match transformation from reference frame" + ); + + let new_position = self.params.transform_position( + &coord.position(), + coord.epoch().to_fractional_year_hardcoded(), + ); + let new_velocity = coord + .velocity() + .as_ref() + .map(|velocity| self.params.transform_velocity(velocity, &coord.position())); + Coordinate::new(self.to, new_position, new_velocity, coord.epoch()) + } + + /// Reverse the transformation + pub fn invert(mut self) -> Self { + std::mem::swap(&mut self.from, &mut self.to); + self.params.invert(); + self + } +} + +#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)] +pub struct TransformationNotFound(ReferenceFrame, ReferenceFrame); + +impl fmt::Display for TransformationNotFound { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!(f, "No transformation found from {} to {}", self.0, self.1) + } +} + +impl std::error::Error for TransformationNotFound {} + +/// Find a transformation from one reference frame to another +/// +/// We currently only support a limited set of transformations. +/// If no transformation is found, `None` is returned. +pub fn get_transformation( + from: ReferenceFrame, + to: ReferenceFrame, +) -> Result { + TRANSFORMATIONS + .iter() + .find(|t| (t.from == from && t.to == to) || (t.from == to && t.to == from)) + .map(|t| { + if t.from == from && t.to == to { + t.clone() + } else { + t.clone().invert() + } + }) + .ok_or(TransformationNotFound(from, to)) +} + +const TRANSFORMATIONS: [Transformation; 9] = [ + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::ITRF2014, + params: TimeDependentHelmertParams { + tx: -1.4, + tx_dot: 0.0, + ty: -0.9, + ty_dot: -0.1, + tz: 1.4, + tz_dot: 0.2, + s: -0.42, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2015.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::ITRF2008, + params: TimeDependentHelmertParams { + tx: 0.2, + tx_dot: 0.0, + ty: 1.0, + ty_dot: -0.1, + tz: 3.3, + tz_dot: 0.1, + s: -0.29, + s_dot: 0.03, + rx: 0.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2015.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::ETRF2020, + params: TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 0.0, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.086, + ry: 0.0, + ry_dot: 0.519, + rz: 0.0, + rz_dot: -0.753, + epoch: 1989.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::ETRF2014, + params: TimeDependentHelmertParams { + tx: -1.4, + tx_dot: 0.0, + ty: -0.9, + ty_dot: -0.1, + tz: 1.4, + tz_dot: 0.2, + s: -0.42, + s_dot: 0.0, + rx: 2.21, + rx_dot: 0.085, + ry: 13.806, + ry_dot: 0.531, + rz: -20.02, + rz_dot: -0.77, + epoch: 2015.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2014, + to: ReferenceFrame::NAD83_2011, + params: TimeDependentHelmertParams { + tx: 1005.30, + tx_dot: 0.79, + ty: -1909.21, + ty_dot: -0.60, + tz: -541.57, + tz_dot: -1.44, + s: 0.36891, + s_dot: -0.07201, + rx: -26.78138, + rx_dot: -0.06667, + ry: 0.42027, + ry_dot: 0.75744, + rz: -10.93206, + rz_dot: 0.05133, + epoch: 2010.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2014, + to: ReferenceFrame::ETRF2014, + params: TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 0.0, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.085, + ry: 0.0, + ry_dot: 0.531, + rz: 0.0, + rz_dot: -0.770, + epoch: 1989.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2008, + to: ReferenceFrame::NAD83_CSRS, + params: TimeDependentHelmertParams { + tx: 1003.70, + tx_dot: 0.79, + ty: -1911.11, + ty_dot: -0.60, + tz: -543.97, + tz_dot: -1.34, + s: 0.38891, + s_dot: -0.10201, + rx: -26.78138, + rx_dot: -0.06667, + ry: 0.42027, + ry_dot: 0.75744, + rz: -10.93206, + rz_dot: 0.05133, + epoch: 2010.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2014, + to: ReferenceFrame::NAD83_CSRS, + params: TimeDependentHelmertParams { + tx: 1005.30, + tx_dot: 0.79, + ty: -1909.21, + ty_dot: -0.60, + tz: -541.57, + tz_dot: -1.44, + s: 0.36891, + s_dot: -0.07201, + rx: -26.78138, + rx_dot: -0.06667, + ry: 0.42027, + ry_dot: 0.75744, + rz: -10.93206, + rz_dot: 0.05133, + epoch: 2010.0, + }, + }, + Transformation { + from: ReferenceFrame::ITRF2020, + to: ReferenceFrame::NAD83_CSRS, + params: TimeDependentHelmertParams { + tx: 1003.90, + tx_dot: 0.79, + ty: -1909.61, + ty_dot: -0.70, + tz: -541.17, + tz_dot: -1.24, + s: -0.05109, + s_dot: -0.07201, + rx: -26.78138, + rx_dot: -0.06667, + ry: 0.42027, + ry_dot: 0.75744, + rz: -10.93206, + rz_dot: 0.05133, + epoch: 2010.0, + }, + }, +]; + +#[cfg(test)] +mod tests { + use float_eq::assert_float_eq; + + #[test] + fn helmert_position_translations() { + let params = super::TimeDependentHelmertParams { + tx: 1.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + tx_dot: 0.1 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + ty: 2.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + ty_dot: 0.2 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + tz: 3.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + tz_dot: 0.3 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + s: 0.0, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2010.0, + }; + let initial_position = super::ECEF::default(); + + let transformed_position = params.transform_position(&initial_position, 2010.0); + assert_float_eq!(transformed_position.x(), 1.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 2.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 3.0, abs_all <= 1e-4); + + let transformed_position = params.transform_position(&initial_position, 2011.0); + assert_float_eq!(transformed_position.x(), 1.1, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 2.2, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 3.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_position_scaling() { + let params = super::TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 1.0 / super::TimeDependentHelmertParams::SCALE_SCALE, + s_dot: 0.1 / super::TimeDependentHelmertParams::SCALE_SCALE, + rx: 90.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2010.0, + }; + let initial_position = super::ECEF::new(1., 2., 3.); + + let transformed_position = params.transform_position(&initial_position, 2010.0); + assert_float_eq!(transformed_position.x(), 2.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 4.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 6.0, abs_all <= 1e-4); + + let transformed_position = params.transform_position(&initial_position, 2011.0); + assert_float_eq!(transformed_position.x(), 2.1, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 4.2, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 6.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_position_rotations() { + let params = super::TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 0.0, + s_dot: 0.0, + rx: 1.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, + rx_dot: 0.1 / super::TimeDependentHelmertParams::ROTATE_SCALE, + ry: 2.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, + ry_dot: 0.2 / super::TimeDependentHelmertParams::ROTATE_SCALE, + rz: 3.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, + rz_dot: 0.3 / super::TimeDependentHelmertParams::ROTATE_SCALE, + epoch: 2010.0, + }; + let initial_position = super::ECEF::new(1.0, 1.0, 1.0); + + let transformed_position = params.transform_position(&initial_position, 2010.0); + assert_float_eq!(transformed_position.x(), 0.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.y(), 3.0, abs_all <= 1e-4); + assert_float_eq!(transformed_position.z(), 0.0, abs_all <= 1e-4); + + let transformed_position = params.transform_position(&initial_position, 2011.0); + assert_float_eq!(transformed_position.x(), -0.1, abs_all <= 1e-9); + assert_float_eq!(transformed_position.y(), 3.2, abs_all <= 1e-9); + assert_float_eq!(transformed_position.z(), -0.1, abs_all <= 1e-9); + } + + #[test] + fn helmert_velocity_translations() { + let params = super::TimeDependentHelmertParams { + tx: 1.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + tx_dot: 0.1 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + ty: 2.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + ty_dot: 0.2 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + tz: 3.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + tz_dot: 0.3 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + s: 0.0, + s_dot: 0.0, + rx: 0.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2010.0, + }; + let initial_velocity = super::ECEF::default(); + let position = super::ECEF::default(); + + let transformed_velocity = params.transform_velocity(&initial_velocity, &position); + assert_float_eq!(transformed_velocity.x(), 0.1, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.y(), 0.2, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.z(), 0.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_velocity_scaling() { + let params = super::TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 1.0 / super::TimeDependentHelmertParams::SCALE_SCALE, + s_dot: 0.1 / super::TimeDependentHelmertParams::SCALE_SCALE, + rx: 90.0, + rx_dot: 0.0, + ry: 0.0, + ry_dot: 0.0, + rz: 0.0, + rz_dot: 0.0, + epoch: 2010.0, + }; + let initial_velocity = super::ECEF::default(); + let position = super::ECEF::new(1., 2., 3.); + + let transformed_velocity = params.transform_velocity(&initial_velocity, &position); + assert_float_eq!(transformed_velocity.x(), 0.1, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.y(), 0.2, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.z(), 0.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_velocity_rotations() { + let params = super::TimeDependentHelmertParams { + tx: 0.0, + tx_dot: 0.0, + ty: 0.0, + ty_dot: 0.0, + tz: 0.0, + tz_dot: 0.0, + s: 0.0, + s_dot: 0.0, + rx: 1.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, + rx_dot: 0.1 / super::TimeDependentHelmertParams::ROTATE_SCALE, + ry: 2.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, + ry_dot: 0.2 / super::TimeDependentHelmertParams::ROTATE_SCALE, + rz: 3.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, + rz_dot: 0.3 / super::TimeDependentHelmertParams::ROTATE_SCALE, + epoch: 2010.0, + }; + let initial_velocity = super::ECEF::default(); + let position = super::ECEF::new(4., 5., 6.); + + let transformed_velocity = params.transform_velocity(&initial_velocity, &position); + assert_float_eq!(transformed_velocity.x(), -0.3, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.y(), 0.6, abs_all <= 1e-4); + assert_float_eq!(transformed_velocity.z(), -0.3, abs_all <= 1e-4); + } + + #[test] + fn helmert_invert() { + let mut params = super::TimeDependentHelmertParams { + tx: 1.0, + tx_dot: 0.1, + ty: 2.0, + ty_dot: 0.2, + tz: 3.0, + tz_dot: 0.3, + s: 4.0, + s_dot: 0.4, + rx: 5.0, + rx_dot: 0.5, + ry: 6.0, + ry_dot: 0.6, + rz: 7.0, + rz_dot: 0.7, + epoch: 2010.0, + }; + params.invert(); + assert_float_eq!(params.tx, -1.0, abs_all <= 1e-4); + assert_float_eq!(params.tx_dot, -0.1, abs_all <= 1e-4); + assert_float_eq!(params.ty, -2.0, abs_all <= 1e-4); + assert_float_eq!(params.ty_dot, -0.2, abs_all <= 1e-4); + assert_float_eq!(params.tz, -3.0, abs_all <= 1e-4); + assert_float_eq!(params.tz_dot, -0.3, abs_all <= 1e-4); + assert_float_eq!(params.s, -4.0, abs_all <= 1e-4); + assert_float_eq!(params.s_dot, -0.4, abs_all <= 1e-4); + assert_float_eq!(params.rx, -5.0, abs_all <= 1e-4); + assert_float_eq!(params.rx_dot, -0.5, abs_all <= 1e-4); + assert_float_eq!(params.ry, -6.0, abs_all <= 1e-4); + assert_float_eq!(params.ry_dot, -0.6, abs_all <= 1e-4); + assert_float_eq!(params.rz, -7.0, abs_all <= 1e-4); + assert_float_eq!(params.rz_dot, -0.7, abs_all <= 1e-4); + assert_float_eq!(params.epoch, 2010.0, abs_all <= 1e-4); + } +} diff --git a/swiftnav/tests/reference_frames.rs b/swiftnav/tests/reference_frames.rs new file mode 100644 index 0000000..fea3637 --- /dev/null +++ b/swiftnav/tests/reference_frames.rs @@ -0,0 +1,380 @@ +use float_eq::assert_float_eq; +use swiftnav::{ + coords::{Coordinate, ECEF}, + reference_frame::{get_transformation, ReferenceFrame}, + time::{GpsTime, UtcTime}, +}; + +fn make_epoch(year: u16) -> GpsTime { + UtcTime::from_date(year, 1, 1, 0, 0, 0.).to_gps_hardcoded() +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_itrf2014() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ITRF2014).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027894.0029, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.6005, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.9063, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0100, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1999, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0302, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ITRF2014); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_itrf2008() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ITRF2008).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027894.0032, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.6023, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.9082, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0101, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1999, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0302, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ITRF2008); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_etrf2020() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ETRF2020).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027894.1545, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.4157, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.7999, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0235, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1832, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0200, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ETRF2020); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_etrf2014() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ETRF2014).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027894.1548, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.4128, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.7937, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0238, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1828, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0200, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ETRF2014); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_etrf2014_reverse() { + let initial_coords = Coordinate::new( + ReferenceFrame::ETRF2014, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ETRF2014, ReferenceFrame::ITRF2020).unwrap(); + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 4027893.8572, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307045.7872, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919475.0263, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + -0.0038, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.2172, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0400, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2000)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ITRF2020); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_adjust_epoch() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let result_coords = initial_coords.adjust_epoch(&make_epoch(2008)); + assert_float_eq!(result_coords.position().x(), 4027894.0860, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307047.2000, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919475.1500, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.01, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.2, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.03, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2008)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ITRF2020); +} + +/// Truth data obtained from https://www.epncb.oma.be/_productsservices/coord_trans/ +#[test] +fn euref_complete_transform() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(4027894.006, 307045.600, 4919474.910), + Some(ECEF::new(0.01, 0.2, 0.030)), + make_epoch(2000), + ); + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::ETRF2014).unwrap(); + + // Test adjusting the epoch first then transforming + let result_coords = transformation.transform(&initial_coords.adjust_epoch(&make_epoch(2008))); + assert_float_eq!(result_coords.position().x(), 4027894.3453, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307046.8755, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.9533, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0238, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1828, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0200, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2008)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ETRF2014); + + // Test transforming first then adjusting the epoch + let result_coords = transformation + .transform(&initial_coords) + .adjust_epoch(&make_epoch(2008)); + assert_float_eq!(result_coords.position().x(), 4027894.3453, abs <= 0.0001); + assert_float_eq!(result_coords.position().y(), 307046.8755, abs <= 0.0001); + assert_float_eq!(result_coords.position().z(), 4919474.9533, abs <= 0.0001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.0238, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.1828, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.0200, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2008)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::ETRF2014); +} + +/// Truth data obtained from https://geodesy.noaa.gov/TOOLS/Htdp/Htdp.shtml +#[test] +fn htdp_nad83_2011_fixed_date() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2014, + ECEF::new(-2705105.358, -4262045.769, 3885381.686), + Some(ECEF::new(-0.02225, 0.02586, 0.01258)), + make_epoch(2010), + ); + + let transformation = + get_transformation(ReferenceFrame::ITRF2014, ReferenceFrame::NAD83_2011).unwrap(); + + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), -2705104.572, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4262047.032, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 3885381.705, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + -0.00594, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.02615, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.02217, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_2011); +} + +/// Truth data obtained from https://geodesy.noaa.gov/TOOLS/Htdp/Htdp.shtml +#[test] +fn htdp_nad83_2011_adjust_epoch() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2014, + ECEF::new(-2705105.358, -4262045.769, 3885381.686), + Some(ECEF::new(-0.02225, 0.02586, 0.01258)), + make_epoch(2020), + ); + + let transformation = + get_transformation(ReferenceFrame::ITRF2014, ReferenceFrame::NAD83_2011).unwrap(); + + let result_coords = transformation.transform(&initial_coords.adjust_epoch(&make_epoch(2010))); + assert_float_eq!(result_coords.position().x(), -2705104.349, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4262047.291, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 3885381.579, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + -0.00594, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.02615, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.02217, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_2011); + + let result_coords = transformation + .transform(&initial_coords) + .adjust_epoch(&make_epoch(2010)); + assert_float_eq!(result_coords.position().x(), -2705104.349, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4262047.291, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 3885381.579, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + -0.00594, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + 0.02615, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + 0.02217, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_2011); +} From 2b1ec0bf2ec84570ffb067a0a345bc3538380734 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Thu, 27 Jun 2024 09:23:33 -0700 Subject: [PATCH 04/11] Add module comments --- swiftnav/src/reference_frame.rs | 80 +++++++++++++++++++++++++++++++-- 1 file changed, 77 insertions(+), 3 deletions(-) diff --git a/swiftnav/src/reference_frame.rs b/swiftnav/src/reference_frame.rs index 11d9ccc..0be9da7 100644 --- a/swiftnav/src/reference_frame.rs +++ b/swiftnav/src/reference_frame.rs @@ -1,3 +1,71 @@ +// Copyright (c) 2024 Swift Navigation Inc. +// Contact: Swift Navigation +// +// This source is subject to the license found in the file 'LICENSE' which must +// be be distributed together with this source. All other rights reserved. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, +// EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. +//! Geodetic reference frame transformations +//! +//! Geodetic reference frames define the coordinate system used to represent +//! positions on the Earth. Different reference frames are commonly used in +//! different regions of the world, and for different purposes. For example, +//! global reference frames, such as the International Terrestrial Reference +//! Frame (ITRF), are used for global positioning, while regional reference +//! frames, such as the European Terrestrial Reference Frame (ETRF), are used +//! for regional positioning. Due to the movement of the earth's crust apparently +//! fixed positions will move over time. Because of this it's important to note +//! only take note of a position, but also the time at which that position was +//! determined. In most regions of the earth the crust moves at a constant speed, +//! meaning that if you are able to determine the local velocity of the crust you +//! can easily determine what the position of a static point would have been in +//! the past. It is commong for regional reference frames to define a common reference +//! epoch that all positions should be transformed to, allowing the direct comparison +//! of positions even if they were determined at different times. Regional reference +//! frames also typically are defined to be "fixed" to a particular tectonic plate, +//! meaning the large majority of the velocity for points on that tectonic plate +//! are cancelled out. In contrast, global reference frames are not fixed to +//! any particular tectonic plate, so most places on earth will have a measurable +//! velocity. Global reference frames also typically do not have a common reference +//! epoch, so determining one's local velocity is important to be able to compare +//! positions or to transform a coordinate from a global reference frame to a regional +//! reference frame. +//! +//! This module provides several types and functions to help transform a set of coordinates +//! from one reference frame to another, and from one epoch to another. Several sets of +//! transformation parameters are included for converting between common reference frames. +//! To start out, you must have a [`Coordinate`](crate::coords::Coordinate) that you want to +//! transform. This consists of a position, an epoch, and a reference frame as well as an optional +//! velocity. You then need to get the [`Transformation`](crate::reference_frame::Transformation) +//! object that describes the transformation from the reference frame of the coordinate to the +//! desired reference frame. You can then call the `transform` method on the transformation object +//! to get a new coordinate in the desired reference frame. This transformation will change the +//! position and velocity of the coordinate, but it does not the change the epoch of the coordinate. +//! If you need to change the epoch of the coordinate you will need to use the [`Coordinate::change_epoch`](crate::coords::Coordinate::change_epoch) +//! method which uses the velocity of the coordinate to determine the position at the new epoch. +//! +//! # Example +//! ``` +//! use swiftnav::{coords::{Coordinate, ECEF}, reference_frame::{ReferenceFrame, get_transformation}, time::UtcTime}; +//! +//! let epoch_2020 = UtcTime::from_date(2020, 3, 15, 0, 0, 0.).to_gps_hardcoded(); +//! let itrf_coord = Coordinate::with_velocity( +//! ReferenceFrame::ITRF2014, // The reference frame of the coordinate +//! ECEF::new(-2703764.0, -4261273.0, 3887158.0), // The position of the coordinate +//! ECEF::new(-0.221, 0.254, 0.122), // The velocity of the coordinate +//! epoch_2020); // The epoch of the coordinate +//! +//! let epoch_2010 = UtcTime::from_date(2010, 1, 1, 0, 0, 0.).to_gps_hardcoded(); +//! let itrf_coord = itrf_coord.adjust_epoch(&epoch_2010); // Change the epoch of the coordinate +//! +//! let transformation = get_transformation(ReferenceFrame::ITRF2014, ReferenceFrame::NAD83_2011).unwrap(); +//! +//! let nad83_coord = transformation.transform(&itrf_coord); +//! ``` +//! + use crate::coords::{Coordinate, ECEF}; use std::{convert::TryFrom, fmt, str::FromStr}; @@ -17,6 +85,10 @@ pub enum ReferenceFrame { NAD83_CSRS, } +/// An Error indicating that the provided reference frame name is invalid +/// +/// This error is returned when trying to convert a string to a [`ReferenceFrame`](crate::reference_frame::ReferenceFrame) +/// and the string does not match any accepted reference frame names. #[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone)] pub struct InvalidReferenceFrameName(String); @@ -169,9 +241,7 @@ impl TimeDependentHelmertParams { } } -/// Container for a complete transformation which includes -/// both the transformation parameters as well as the source -/// and destination reference frames +/// A transformation from one reference frame to another. #[derive(Debug, PartialEq, PartialOrd, Clone)] pub struct Transformation { from: ReferenceFrame, @@ -209,6 +279,10 @@ impl Transformation { } } +/// Error indicating that no transformation was found between two reference frames +/// +/// This error is returned when trying to find a transformation between two reference frames +/// and no transformation is found. #[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)] pub struct TransformationNotFound(ReferenceFrame, ReferenceFrame); From 4af02ffc390de67f3ad95ef5752158d1fbf8bde2 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Thu, 27 Jun 2024 09:56:44 -0700 Subject: [PATCH 05/11] Make the doc example a little more clear --- swiftnav/src/reference_frame.rs | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/swiftnav/src/reference_frame.rs b/swiftnav/src/reference_frame.rs index 0be9da7..ec54569 100644 --- a/swiftnav/src/reference_frame.rs +++ b/swiftnav/src/reference_frame.rs @@ -48,7 +48,14 @@ //! //! # Example //! ``` -//! use swiftnav::{coords::{Coordinate, ECEF}, reference_frame::{ReferenceFrame, get_transformation}, time::UtcTime}; +//! use swiftnav::{ +//! coords::{Coordinate, ECEF}, +//! reference_frame::{ReferenceFrame, get_transformation}, +//! time::UtcTime +//! }; +//! +//! let transformation = get_transformation(ReferenceFrame::ITRF2014, ReferenceFrame::NAD83_2011) +//! .unwrap(); //! //! let epoch_2020 = UtcTime::from_date(2020, 3, 15, 0, 0, 0.).to_gps_hardcoded(); //! let itrf_coord = Coordinate::with_velocity( @@ -60,9 +67,10 @@ //! let epoch_2010 = UtcTime::from_date(2010, 1, 1, 0, 0, 0.).to_gps_hardcoded(); //! let itrf_coord = itrf_coord.adjust_epoch(&epoch_2010); // Change the epoch of the coordinate //! -//! let transformation = get_transformation(ReferenceFrame::ITRF2014, ReferenceFrame::NAD83_2011).unwrap(); -//! //! let nad83_coord = transformation.transform(&itrf_coord); +//! // Alternatively, you can use the `transform_to` method on the coordinate itself +//! let nad83_coord: Result = +//! itrf_coord.transform_to(ReferenceFrame::NAD83_2011); //! ``` //! From 4e6d5992fe8ddd33a680375cb928b16cad85f340 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Thu, 27 Jun 2024 09:57:54 -0700 Subject: [PATCH 06/11] Fix doc comment link --- swiftnav/src/reference_frame.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/swiftnav/src/reference_frame.rs b/swiftnav/src/reference_frame.rs index ec54569..b403091 100644 --- a/swiftnav/src/reference_frame.rs +++ b/swiftnav/src/reference_frame.rs @@ -43,7 +43,7 @@ //! desired reference frame. You can then call the `transform` method on the transformation object //! to get a new coordinate in the desired reference frame. This transformation will change the //! position and velocity of the coordinate, but it does not the change the epoch of the coordinate. -//! If you need to change the epoch of the coordinate you will need to use the [`Coordinate::change_epoch`](crate::coords::Coordinate::change_epoch) +//! If you need to change the epoch of the coordinate you will need to use the [`Coordinate::adjust_epoch`](crate::coords::Coordinate::adjust_epoch) //! method which uses the velocity of the coordinate to determine the position at the new epoch. //! //! # Example From 7f88fe5d1a3f58d7396a178eb0527a2fae0574e7 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Thu, 27 Jun 2024 09:59:55 -0700 Subject: [PATCH 07/11] Fix test failure --- swiftnav/src/reference_frame.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/swiftnav/src/reference_frame.rs b/swiftnav/src/reference_frame.rs index b403091..c7448e4 100644 --- a/swiftnav/src/reference_frame.rs +++ b/swiftnav/src/reference_frame.rs @@ -50,7 +50,7 @@ //! ``` //! use swiftnav::{ //! coords::{Coordinate, ECEF}, -//! reference_frame::{ReferenceFrame, get_transformation}, +//! reference_frame::{get_transformation, ReferenceFrame, TransformationNotFound}, //! time::UtcTime //! }; //! From b93ea81c5ec1b3a669d7ad6463cc50696d8b13ba Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Thu, 27 Jun 2024 16:58:58 -0700 Subject: [PATCH 08/11] PR feedback: Use strum for enum string operations, and add additional test --- swiftnav/Cargo.toml | 1 + swiftnav/src/reference_frame.rs | 230 +++++++++++++++++--------------- 2 files changed, 121 insertions(+), 110 deletions(-) diff --git a/swiftnav/Cargo.toml b/swiftnav/Cargo.toml index b77216a..ff1b90d 100644 --- a/swiftnav/Cargo.toml +++ b/swiftnav/Cargo.toml @@ -13,6 +13,7 @@ rust-version = "1.62.1" rustversion = "1.0" chrono = { version = "0.4", optional = true } swiftnav-sys = { version = "^0.8.1", path = "../swiftnav-sys/" } +strum = { version = "0.26", features = ["derive"] } [dev-dependencies] float_eq = "1.0.1" diff --git a/swiftnav/src/reference_frame.rs b/swiftnav/src/reference_frame.rs index c7448e4..a215de6 100644 --- a/swiftnav/src/reference_frame.rs +++ b/swiftnav/src/reference_frame.rs @@ -75,10 +75,12 @@ //! use crate::coords::{Coordinate, ECEF}; -use std::{convert::TryFrom, fmt, str::FromStr}; +use std::fmt; +use strum::{Display, EnumIter, EnumString}; /// Reference Frames -#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy, EnumString, Display, EnumIter)] +#[strum(serialize_all = "UPPERCASE")] pub enum ReferenceFrame { ITRF2008, ITRF2014, @@ -87,76 +89,14 @@ pub enum ReferenceFrame { ETRF2014, ETRF2020, /// i.e. NAD83(2011) + #[strum(to_string = "NAD83(2011)", serialize = "NAD83_2011")] NAD83_2011, /// i.e. NAD83(CSRS) - Canadian Spatial Reference System #[allow(non_camel_case_types)] + #[strum(to_string = "NAD83(CSRS)", serialize = "NAD83_CSRS")] NAD83_CSRS, } -/// An Error indicating that the provided reference frame name is invalid -/// -/// This error is returned when trying to convert a string to a [`ReferenceFrame`](crate::reference_frame::ReferenceFrame) -/// and the string does not match any accepted reference frame names. -#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone)] -pub struct InvalidReferenceFrameName(String); - -impl fmt::Display for InvalidReferenceFrameName { - fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - write!(f, "Invalid reference frame name ({})", self.0) - } -} - -impl std::error::Error for InvalidReferenceFrameName {} - -impl TryFrom<&str> for ReferenceFrame { - type Error = InvalidReferenceFrameName; - - fn try_from(value: &str) -> Result { - match value { - "ITRF2008" => Ok(ReferenceFrame::ITRF2008), - "ITRF2014" => Ok(ReferenceFrame::ITRF2014), - "ITRF2020" => Ok(ReferenceFrame::ITRF2020), - "ETRF2008" => Ok(ReferenceFrame::ETRF2008), - "ETRF2014" => Ok(ReferenceFrame::ETRF2014), - "ETRF2020" => Ok(ReferenceFrame::ETRF2020), - "NAD83(2011)" => Ok(ReferenceFrame::NAD83_2011), - "NAD83_2011" => Ok(ReferenceFrame::NAD83_2011), - "NAD83(CSRS)" => Ok(ReferenceFrame::NAD83_CSRS), - "NAD83_CSRS" => Ok(ReferenceFrame::NAD83_CSRS), - _ => Err(InvalidReferenceFrameName(value.to_string())), - } - } -} - -impl FromStr for ReferenceFrame { - type Err = InvalidReferenceFrameName; - - fn from_str(s: &str) -> Result { - ReferenceFrame::try_from(s) - } -} - -impl From for &'static str { - fn from(value: ReferenceFrame) -> &'static str { - match value { - ReferenceFrame::ITRF2008 => "ITRF2008", - ReferenceFrame::ITRF2014 => "ITRF2014", - ReferenceFrame::ITRF2020 => "ITRF2020", - ReferenceFrame::ETRF2008 => "ETRF2008", - ReferenceFrame::ETRF2014 => "ETRF2014", - ReferenceFrame::ETRF2020 => "ETRF2020", - ReferenceFrame::NAD83_2011 => "NAD83(2011)", - ReferenceFrame::NAD83_CSRS => "NAD83(CSRS)", - } - } -} - -impl fmt::Display for ReferenceFrame { - fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - write!(f, "{}", Into::<&'static str>::into(*self)) - } -} - /// 15-parameter Helmert transformation parameters /// /// This transformation consists of a 3 dimensional translation, @@ -517,17 +457,71 @@ const TRANSFORMATIONS: [Transformation; 9] = [ #[cfg(test)] mod tests { + use super::*; use float_eq::assert_float_eq; + use std::str::FromStr; + + #[test] + fn reference_frame_strings() { + assert_eq!(ReferenceFrame::ITRF2008.to_string(), "ITRF2008"); + assert_eq!( + ReferenceFrame::from_str("ITRF2008"), + Ok(ReferenceFrame::ITRF2008) + ); + assert_eq!(ReferenceFrame::ITRF2014.to_string(), "ITRF2014"); + assert_eq!( + ReferenceFrame::from_str("ITRF2014"), + Ok(ReferenceFrame::ITRF2014) + ); + assert_eq!(ReferenceFrame::ITRF2020.to_string(), "ITRF2020"); + assert_eq!( + ReferenceFrame::from_str("ITRF2020"), + Ok(ReferenceFrame::ITRF2020) + ); + assert_eq!(ReferenceFrame::ETRF2008.to_string(), "ETRF2008"); + assert_eq!( + ReferenceFrame::from_str("ETRF2008"), + Ok(ReferenceFrame::ETRF2008) + ); + assert_eq!(ReferenceFrame::ETRF2014.to_string(), "ETRF2014"); + assert_eq!( + ReferenceFrame::from_str("ETRF2014"), + Ok(ReferenceFrame::ETRF2014) + ); + assert_eq!(ReferenceFrame::ETRF2020.to_string(), "ETRF2020"); + assert_eq!( + ReferenceFrame::from_str("ETRF2020"), + Ok(ReferenceFrame::ETRF2020) + ); + assert_eq!(ReferenceFrame::NAD83_2011.to_string(), "NAD83(2011)"); + assert_eq!( + ReferenceFrame::from_str("NAD83_2011"), + Ok(ReferenceFrame::NAD83_2011) + ); + assert_eq!( + ReferenceFrame::from_str("NAD83(2011)"), + Ok(ReferenceFrame::NAD83_2011) + ); + assert_eq!(ReferenceFrame::NAD83_CSRS.to_string(), "NAD83(CSRS)"); + assert_eq!( + ReferenceFrame::from_str("NAD83_CSRS"), + Ok(ReferenceFrame::NAD83_CSRS) + ); + assert_eq!( + ReferenceFrame::from_str("NAD83(CSRS)"), + Ok(ReferenceFrame::NAD83_CSRS) + ); + } #[test] fn helmert_position_translations() { - let params = super::TimeDependentHelmertParams { - tx: 1.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - tx_dot: 0.1 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - ty: 2.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - ty_dot: 0.2 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - tz: 3.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - tz_dot: 0.3 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + let params = TimeDependentHelmertParams { + tx: 1.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tx_dot: 0.1 / TimeDependentHelmertParams::TRANSLATE_SCALE, + ty: 2.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + ty_dot: 0.2 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tz: 3.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tz_dot: 0.3 / TimeDependentHelmertParams::TRANSLATE_SCALE, s: 0.0, s_dot: 0.0, rx: 0.0, @@ -538,7 +532,7 @@ mod tests { rz_dot: 0.0, epoch: 2010.0, }; - let initial_position = super::ECEF::default(); + let initial_position = ECEF::default(); let transformed_position = params.transform_position(&initial_position, 2010.0); assert_float_eq!(transformed_position.x(), 1.0, abs_all <= 1e-4); @@ -553,15 +547,15 @@ mod tests { #[test] fn helmert_position_scaling() { - let params = super::TimeDependentHelmertParams { + let params = TimeDependentHelmertParams { tx: 0.0, tx_dot: 0.0, ty: 0.0, ty_dot: 0.0, tz: 0.0, tz_dot: 0.0, - s: 1.0 / super::TimeDependentHelmertParams::SCALE_SCALE, - s_dot: 0.1 / super::TimeDependentHelmertParams::SCALE_SCALE, + s: 1.0 / TimeDependentHelmertParams::SCALE_SCALE, + s_dot: 0.1 / TimeDependentHelmertParams::SCALE_SCALE, rx: 90.0, rx_dot: 0.0, ry: 0.0, @@ -570,7 +564,7 @@ mod tests { rz_dot: 0.0, epoch: 2010.0, }; - let initial_position = super::ECEF::new(1., 2., 3.); + let initial_position = ECEF::new(1., 2., 3.); let transformed_position = params.transform_position(&initial_position, 2010.0); assert_float_eq!(transformed_position.x(), 2.0, abs_all <= 1e-4); @@ -585,7 +579,7 @@ mod tests { #[test] fn helmert_position_rotations() { - let params = super::TimeDependentHelmertParams { + let params = TimeDependentHelmertParams { tx: 0.0, tx_dot: 0.0, ty: 0.0, @@ -594,15 +588,15 @@ mod tests { tz_dot: 0.0, s: 0.0, s_dot: 0.0, - rx: 1.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, - rx_dot: 0.1 / super::TimeDependentHelmertParams::ROTATE_SCALE, - ry: 2.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, - ry_dot: 0.2 / super::TimeDependentHelmertParams::ROTATE_SCALE, - rz: 3.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, - rz_dot: 0.3 / super::TimeDependentHelmertParams::ROTATE_SCALE, + rx: 1.0 / TimeDependentHelmertParams::ROTATE_SCALE, + rx_dot: 0.1 / TimeDependentHelmertParams::ROTATE_SCALE, + ry: 2.0 / TimeDependentHelmertParams::ROTATE_SCALE, + ry_dot: 0.2 / TimeDependentHelmertParams::ROTATE_SCALE, + rz: 3.0 / TimeDependentHelmertParams::ROTATE_SCALE, + rz_dot: 0.3 / TimeDependentHelmertParams::ROTATE_SCALE, epoch: 2010.0, }; - let initial_position = super::ECEF::new(1.0, 1.0, 1.0); + let initial_position = ECEF::new(1.0, 1.0, 1.0); let transformed_position = params.transform_position(&initial_position, 2010.0); assert_float_eq!(transformed_position.x(), 0.0, abs_all <= 1e-4); @@ -617,13 +611,13 @@ mod tests { #[test] fn helmert_velocity_translations() { - let params = super::TimeDependentHelmertParams { - tx: 1.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - tx_dot: 0.1 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - ty: 2.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - ty_dot: 0.2 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - tz: 3.0 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, - tz_dot: 0.3 / super::TimeDependentHelmertParams::TRANSLATE_SCALE, + let params = TimeDependentHelmertParams { + tx: 1.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tx_dot: 0.1 / TimeDependentHelmertParams::TRANSLATE_SCALE, + ty: 2.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + ty_dot: 0.2 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tz: 3.0 / TimeDependentHelmertParams::TRANSLATE_SCALE, + tz_dot: 0.3 / TimeDependentHelmertParams::TRANSLATE_SCALE, s: 0.0, s_dot: 0.0, rx: 0.0, @@ -634,8 +628,8 @@ mod tests { rz_dot: 0.0, epoch: 2010.0, }; - let initial_velocity = super::ECEF::default(); - let position = super::ECEF::default(); + let initial_velocity = ECEF::default(); + let position = ECEF::default(); let transformed_velocity = params.transform_velocity(&initial_velocity, &position); assert_float_eq!(transformed_velocity.x(), 0.1, abs_all <= 1e-4); @@ -645,15 +639,15 @@ mod tests { #[test] fn helmert_velocity_scaling() { - let params = super::TimeDependentHelmertParams { + let params = TimeDependentHelmertParams { tx: 0.0, tx_dot: 0.0, ty: 0.0, ty_dot: 0.0, tz: 0.0, tz_dot: 0.0, - s: 1.0 / super::TimeDependentHelmertParams::SCALE_SCALE, - s_dot: 0.1 / super::TimeDependentHelmertParams::SCALE_SCALE, + s: 1.0 / TimeDependentHelmertParams::SCALE_SCALE, + s_dot: 0.1 / TimeDependentHelmertParams::SCALE_SCALE, rx: 90.0, rx_dot: 0.0, ry: 0.0, @@ -662,8 +656,8 @@ mod tests { rz_dot: 0.0, epoch: 2010.0, }; - let initial_velocity = super::ECEF::default(); - let position = super::ECEF::new(1., 2., 3.); + let initial_velocity = ECEF::default(); + let position = ECEF::new(1., 2., 3.); let transformed_velocity = params.transform_velocity(&initial_velocity, &position); assert_float_eq!(transformed_velocity.x(), 0.1, abs_all <= 1e-4); @@ -673,7 +667,7 @@ mod tests { #[test] fn helmert_velocity_rotations() { - let params = super::TimeDependentHelmertParams { + let params = TimeDependentHelmertParams { tx: 0.0, tx_dot: 0.0, ty: 0.0, @@ -682,16 +676,16 @@ mod tests { tz_dot: 0.0, s: 0.0, s_dot: 0.0, - rx: 1.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, - rx_dot: 0.1 / super::TimeDependentHelmertParams::ROTATE_SCALE, - ry: 2.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, - ry_dot: 0.2 / super::TimeDependentHelmertParams::ROTATE_SCALE, - rz: 3.0 / super::TimeDependentHelmertParams::ROTATE_SCALE, - rz_dot: 0.3 / super::TimeDependentHelmertParams::ROTATE_SCALE, + rx: 1.0 / TimeDependentHelmertParams::ROTATE_SCALE, + rx_dot: 0.1 / TimeDependentHelmertParams::ROTATE_SCALE, + ry: 2.0 / TimeDependentHelmertParams::ROTATE_SCALE, + ry_dot: 0.2 / TimeDependentHelmertParams::ROTATE_SCALE, + rz: 3.0 / TimeDependentHelmertParams::ROTATE_SCALE, + rz_dot: 0.3 / TimeDependentHelmertParams::ROTATE_SCALE, epoch: 2010.0, }; - let initial_velocity = super::ECEF::default(); - let position = super::ECEF::new(4., 5., 6.); + let initial_velocity = ECEF::default(); + let position = ECEF::new(4., 5., 6.); let transformed_velocity = params.transform_velocity(&initial_velocity, &position); assert_float_eq!(transformed_velocity.x(), -0.3, abs_all <= 1e-4); @@ -701,7 +695,7 @@ mod tests { #[test] fn helmert_invert() { - let mut params = super::TimeDependentHelmertParams { + let mut params = TimeDependentHelmertParams { tx: 1.0, tx_dot: 0.1, ty: 2.0, @@ -734,5 +728,21 @@ mod tests { assert_float_eq!(params.rz, -7.0, abs_all <= 1e-4); assert_float_eq!(params.rz_dot, -0.7, abs_all <= 1e-4); assert_float_eq!(params.epoch, 2010.0, abs_all <= 1e-4); + params.invert(); + assert_float_eq!(params.tx, 1.0, abs_all <= 1e-4); + assert_float_eq!(params.tx_dot, 0.1, abs_all <= 1e-4); + assert_float_eq!(params.ty, 2.0, abs_all <= 1e-4); + assert_float_eq!(params.ty_dot, 0.2, abs_all <= 1e-4); + assert_float_eq!(params.tz, 3.0, abs_all <= 1e-4); + assert_float_eq!(params.tz_dot, 0.3, abs_all <= 1e-4); + assert_float_eq!(params.s, 4.0, abs_all <= 1e-4); + assert_float_eq!(params.s_dot, 0.4, abs_all <= 1e-4); + assert_float_eq!(params.rx, 5.0, abs_all <= 1e-4); + assert_float_eq!(params.rx_dot, 0.5, abs_all <= 1e-4); + assert_float_eq!(params.ry, 6.0, abs_all <= 1e-4); + assert_float_eq!(params.ry_dot, 0.6, abs_all <= 1e-4); + assert_float_eq!(params.rz, 7.0, abs_all <= 1e-4); + assert_float_eq!(params.rz_dot, 0.7, abs_all <= 1e-4); + assert_float_eq!(params.epoch, 2010.0, abs_all <= 1e-4); } } From 5d8d0801d0155c6a42f0cef03b77688e90f08373 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Fri, 28 Jun 2024 10:04:35 -0700 Subject: [PATCH 09/11] Add some Copy derivations --- swiftnav/src/coords.rs | 2 +- swiftnav/src/reference_frame.rs | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/swiftnav/src/coords.rs b/swiftnav/src/coords.rs index 81369e3..c37e403 100644 --- a/swiftnav/src/coords.rs +++ b/swiftnav/src/coords.rs @@ -525,7 +525,7 @@ impl Default for AzimuthElevation { /// Complete coordinate used for transforming between reference frames /// /// Velocities are optional, but when present they will be transformed -#[derive(Debug, PartialEq, PartialOrd, Clone)] +#[derive(Debug, PartialEq, PartialOrd, Clone, Copy)] pub struct Coordinate { reference_frame: ReferenceFrame, position: ECEF, diff --git a/swiftnav/src/reference_frame.rs b/swiftnav/src/reference_frame.rs index a215de6..d235452 100644 --- a/swiftnav/src/reference_frame.rs +++ b/swiftnav/src/reference_frame.rs @@ -111,7 +111,7 @@ pub enum ReferenceFrame { /// millimeters, the rotations are in milliarcseconds, and /// the scaling is in parts per billion. We also follow the /// IERS convention for the sign of the rotation terms. -#[derive(Debug, PartialEq, PartialOrd, Clone)] +#[derive(Debug, PartialEq, PartialOrd, Clone, Copy)] struct TimeDependentHelmertParams { tx: f64, tx_dot: f64, @@ -190,7 +190,7 @@ impl TimeDependentHelmertParams { } /// A transformation from one reference frame to another. -#[derive(Debug, PartialEq, PartialOrd, Clone)] +#[derive(Debug, PartialEq, PartialOrd, Clone, Copy)] pub struct Transformation { from: ReferenceFrame, to: ReferenceFrame, From 93ae426ffe27d135b4578f3853e212dc36f1ede1 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Fri, 28 Jun 2024 10:37:45 -0700 Subject: [PATCH 10/11] Fix clippy errors --- swiftnav/src/reference_frame.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/swiftnav/src/reference_frame.rs b/swiftnav/src/reference_frame.rs index d235452..bdb18e8 100644 --- a/swiftnav/src/reference_frame.rs +++ b/swiftnav/src/reference_frame.rs @@ -255,9 +255,9 @@ pub fn get_transformation( .find(|t| (t.from == from && t.to == to) || (t.from == to && t.to == from)) .map(|t| { if t.from == from && t.to == to { - t.clone() + *t } else { - t.clone().invert() + (*t).invert() } }) .ok_or(TransformationNotFound(from, to)) From 2d4aba9ea074bc2c3c1b2771e6f35f1c61a8e976 Mon Sep 17 00:00:00 2001 From: Joseph Angelo Date: Fri, 28 Jun 2024 10:38:26 -0700 Subject: [PATCH 11/11] Add integration tests for NAD83(CSRS) --- swiftnav/tests/reference_frames.rs | 99 ++++++++++++++++++++++++++++++ 1 file changed, 99 insertions(+) diff --git a/swiftnav/tests/reference_frames.rs b/swiftnav/tests/reference_frames.rs index fea3637..2a23548 100644 --- a/swiftnav/tests/reference_frames.rs +++ b/swiftnav/tests/reference_frames.rs @@ -378,3 +378,102 @@ fn htdp_nad83_2011_adjust_epoch() { assert_eq!(result_coords.epoch(), make_epoch(2010)); assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_2011); } + +/// Truth data obtained from https://webapp.csrs-scrs.nrcan-rncan.gc.ca/geod/tools-outils/trx.php +#[test] +fn trx_nad83_csrs_fixed_date() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(1267458.677, -4294620.216, 4526843.210), + Some(ECEF::new(-0.01578, -0.00380, 0.00466)), + make_epoch(2010), + ); + + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::NAD83_CSRS).unwrap(); + + let result_coords = transformation.transform(&initial_coords); + assert_float_eq!(result_coords.position().x(), 1267459.462, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4294621.605, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 4526843.224, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.00261, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + -0.00241, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + -0.00017, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_CSRS); +} + +/// Truth data obtained from https://geodesy.noaa.gov/TOOLS/Htdp/Htdp.shtml +#[test] +fn trx_nad83_csrs_adjust_epoch() { + let initial_coords = Coordinate::new( + ReferenceFrame::ITRF2020, + ECEF::new(1267458.677, -4294620.216, 4526843.210), + Some(ECEF::new(-0.01578, -0.00380, 0.00466)), + make_epoch(2020), + ); + + let transformation = + get_transformation(ReferenceFrame::ITRF2020, ReferenceFrame::NAD83_CSRS).unwrap(); + + let result_coords = transformation.transform(&initial_coords.adjust_epoch(&make_epoch(2010))); + assert_float_eq!(result_coords.position().x(), 1267459.620, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4294621.567, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 4526843.177, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.00261, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + -0.00241, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + -0.00017, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_CSRS); + + let result_coords = transformation + .transform(&initial_coords) + .adjust_epoch(&make_epoch(2010)); + assert_float_eq!(result_coords.position().x(), 1267459.620, abs <= 0.001); + assert_float_eq!(result_coords.position().y(), -4294621.567, abs <= 0.001); + assert_float_eq!(result_coords.position().z(), 4526843.177, abs <= 0.001); + assert!(result_coords.velocity().is_some()); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().x(), + 0.00261, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().y(), + -0.00241, + abs <= 0.1 + ); + assert_float_eq!( + result_coords.velocity().as_ref().unwrap().z(), + -0.00017, + abs <= 0.1 + ); + assert_eq!(result_coords.epoch(), make_epoch(2010)); + assert_eq!(result_coords.reference_frame(), ReferenceFrame::NAD83_CSRS); +}