From 65f0902582e6bc5eb87db05602f035a14e37dfb6 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 13 Sep 2024 16:17:54 -0600 Subject: [PATCH 1/2] Add state differencing functionality + VNC and RCN frames include time derivative of the rotation matrix - RSS radius and velocity tests - VNC differencing - Absolute and relative state differencing --- anise/src/almanac/aer.rs | 3 +- anise/src/astro/orbit.rs | 95 ++++++++++++++++++++++++++++++- anise/src/math/cartesian.rs | 109 +++++++++++++++++++++++++++++++++++- anise/src/math/mod.rs | 19 ++++++- anise/tests/astro/orbit.rs | 80 ++++++++++++++++++++++++-- 5 files changed, 293 insertions(+), 13 deletions(-) diff --git a/anise/src/almanac/aer.rs b/anise/src/almanac/aer.rs index dc922cf5..9dc650e1 100644 --- a/anise/src/almanac/aer.rs +++ b/anise/src/almanac/aer.rs @@ -15,6 +15,7 @@ use crate::{ frames::Frame, math::angles::{between_0_360, between_pm_180}, prelude::Orbit, + time::uuid_from_epoch, }; use super::Almanac; @@ -70,7 +71,7 @@ impl Almanac { } // Compute the SEZ DCM - let from = tx.frame.orientation_id * 1_000 + 1; + let from = uuid_from_epoch(tx.frame.orientation_id, rx.epoch); // SEZ DCM is topo to fixed let sez_dcm = tx .dcm_from_topocentric_to_body_fixed(from) diff --git a/anise/src/astro/orbit.rs b/anise/src/astro/orbit.rs index 6447f5fa..df6d8551 100644 --- a/anise/src/astro/orbit.rs +++ b/anise/src/astro/orbit.rs @@ -425,7 +425,7 @@ impl Orbit { /// 2. Compute the cross product of these /// 3. Build the DCM with these unit vectors /// 4. Return the DCM structure - pub fn dcm_from_rcn_to_inertial(&self) -> PhysicsResult { + pub fn dcm3x3_from_rcn_to_inertial(&self) -> PhysicsResult { let r = self.r_hat(); let n = self.hvec()? / self.hmag()?; let c = n.cross(&r); @@ -440,6 +440,41 @@ impl Orbit { }) } + /// Builds the rotation matrix that rotates from this state's inertial frame to this state's RCN frame (radial, cross, normal) + /// + /// # Frame warning + /// If the stattion is NOT in an inertial frame, then this computation is INVALID. + /// + /// # Algorithm + /// 1. Compute \hat{r}, \hat{h}, the unit vectors of the radius and orbital momentum. + /// 2. Compute the cross product of these + /// 3. Build the DCM with these unit vectors + /// 4. Return the DCM structure with a 6x6 DCM with the time derivative of the VNC frame set. + /// + /// # Note on the time derivative + /// If the pre or post states cannot be computed, then the time derivative of the DCM will _not_ be set. + /// Further note that most astrodynamics tools do *not* account for the time derivative in the RIC frame. + pub fn dcm_from_rcn_to_inertial(&self) -> PhysicsResult { + let rot_mat_dt = if let Ok(pre) = self.at_epoch(self.epoch - Unit::Millisecond * 1) { + if let Ok(post) = self.at_epoch(self.epoch + Unit::Millisecond * 1) { + let dcm_pre = pre.dcm3x3_from_rcn_to_inertial()?; + let dcm_post = post.dcm3x3_from_rcn_to_inertial()?; + Some(0.5 * dcm_post.rot_mat - 0.5 * dcm_pre.rot_mat) + } else { + None + } + } else { + None + }; + + Ok(DCM { + rot_mat: self.dcm3x3_from_rcn_to_inertial()?.rot_mat, + rot_mat_dt, + from: uuid_from_epoch(self.frame.orientation_id, self.epoch), + to: self.frame.orientation_id, + }) + } + /// Builds the rotation matrix that rotates from this state's inertial frame to this state's VNC frame (velocity, normal, cross) /// /// # Frame warning @@ -449,8 +484,9 @@ impl Orbit { /// 1. Compute \hat{v}, \hat{h}, the unit vectors of the radius and orbital momentum. /// 2. Compute the cross product of these /// 3. Build the DCM with these unit vectors - /// 4. Return the DCM structure - pub fn dcm_from_vnc_to_inertial(&self) -> PhysicsResult { + /// 4. Return the DCM structure. + /// + pub fn dcm3x3_from_vnc_to_inertial(&self) -> PhysicsResult { let v = self.velocity_km_s / self.vmag_km_s(); let n = self.hvec()? / self.hmag()?; let c = v.cross(&n); @@ -464,6 +500,42 @@ impl Orbit { to: self.frame.orientation_id, }) } + + /// Builds the rotation matrix that rotates from this state's inertial frame to this state's VNC frame (velocity, normal, cross) + /// + /// # Frame warning + /// If the stattion is NOT in an inertial frame, then this computation is INVALID. + /// + /// # Algorithm + /// 1. Compute \hat{v}, \hat{h}, the unit vectors of the radius and orbital momentum. + /// 2. Compute the cross product of these + /// 3. Build the DCM with these unit vectors + /// 4. Compute the difference between the DCMs of the pre and post states (+/- 1 ms), to build the DCM time derivative + /// 4. Return the DCM structure with a 6x6 DCM with the time derivative of the VNC frame set. + /// + /// # Note on the time derivative + /// If the pre or post states cannot be computed, then the time derivative of the DCM will _not_ be set. + /// Further note that most astrodynamics tools do *not* account for the time derivative in the RIC frame. + pub fn dcm_from_vnc_to_inertial(&self) -> PhysicsResult { + let rot_mat_dt = if let Ok(pre) = self.at_epoch(self.epoch - Unit::Millisecond * 1) { + if let Ok(post) = self.at_epoch(self.epoch + Unit::Millisecond * 1) { + let dcm_pre = pre.dcm3x3_from_vnc_to_inertial()?; + let dcm_post = post.dcm3x3_from_vnc_to_inertial()?; + Some(0.5 * dcm_post.rot_mat - 0.5 * dcm_pre.rot_mat) + } else { + None + } + } else { + None + }; + + Ok(DCM { + rot_mat: self.dcm3x3_from_vnc_to_inertial()?.rot_mat, + rot_mat_dt, + from: uuid_from_epoch(self.frame.orientation_id, self.epoch), + to: self.frame.orientation_id, + }) + } } #[allow(clippy::too_many_arguments)] @@ -1110,6 +1182,23 @@ impl Orbit { rslt.frame.strip(); Ok(rslt) } + + /// Returns a Cartesian state representing the VNC difference between self and other, in position and velocity (with transport theorem). + /// Refer to dcm_from_vnc_to_inertial for details on the VNC frame. + /// + /// # Algorithm + /// 1. Compute the VNC DCM of self + /// 2. Rotate self into the VNC frame + /// 3. Rotation other into the VNC frame + /// 4. Compute the difference between these two states + /// 5. Strip the astrodynamical information from the frame, enabling only computations from `CartesianState` + pub fn vnc_difference(&self, other: &Self) -> PhysicsResult { + let self_in_vnc = (self.dcm_from_vnc_to_inertial()?.transpose() * self)?; + let other_in_vnc = (self.dcm_from_vnc_to_inertial()?.transpose() * other)?; + let mut rslt = (self_in_vnc - other_in_vnc)?; + rslt.frame.strip(); + Ok(rslt) + } } #[allow(clippy::format_in_format_args)] diff --git a/anise/src/math/cartesian.rs b/anise/src/math/cartesian.rs index d0b06ec6..0908f6bd 100644 --- a/anise/src/math/cartesian.rs +++ b/anise/src/math/cartesian.rs @@ -8,11 +8,11 @@ * Documentation: https://nyxspace.com/ */ -use super::{perp_vector, root_mean_squared, Vector3}; +use super::{perp_vector, root_mean_squared, root_sum_squared, Vector3}; use crate::{ astro::PhysicsResult, constants::SPEED_OF_LIGHT_KM_S, - errors::{EpochMismatchSnafu, FrameMismatchSnafu, PhysicsError}, + errors::{EpochMismatchSnafu, FrameMismatchSnafu, MathError, PhysicsError}, prelude::Frame, }; @@ -253,11 +253,39 @@ impl CartesianState { frame2: other.frame } ); - Ok(root_mean_squared(&self.radius_km, &other.radius_km)) + Ok(root_sum_squared(&self.radius_km, &other.radius_km)) } /// Returns the root mean squared (RSS) velocity difference between this state and another state, if both frames match (epoch does not need to match) pub fn rss_velocity_km_s(&self, other: &Self) -> PhysicsResult { + ensure!( + self.frame.ephem_origin_match(other.frame) + && self.frame.orient_origin_match(other.frame), + FrameMismatchSnafu { + action: "computing velocity RSS", + frame1: self.frame, + frame2: other.frame + } + ); + Ok(root_sum_squared(&self.velocity_km_s, &other.velocity_km_s)) + } + + /// Returns the root sum squared (RMS) radius difference between this state and another state, if both frames match (epoch does not need to match) + pub fn rms_radius_km(&self, other: &Self) -> PhysicsResult { + ensure!( + self.frame.ephem_origin_match(other.frame) + && self.frame.orient_origin_match(other.frame), + FrameMismatchSnafu { + action: "computing radius RSS", + frame1: self.frame, + frame2: other.frame + } + ); + Ok(root_mean_squared(&self.radius_km, &other.radius_km)) + } + + /// Returns the root sum squared (RMS) velocity difference between this state and another state, if both frames match (epoch does not need to match) + pub fn rms_velocity_km_s(&self, other: &Self) -> PhysicsResult { ensure!( self.frame.ephem_origin_match(other.frame) && self.frame.orient_origin_match(other.frame), @@ -287,6 +315,81 @@ impl CartesianState { pub fn light_time(&self) -> Duration { (self.radius_km.norm() / SPEED_OF_LIGHT_KM_S).seconds() } + + /// Returns the absolute position difference in kilometer between this orbit and another. + /// Raises an error if the frames do not match (epochs do not need to match). + pub fn abs_pos_diff_km(&self, other: &Self) -> PhysicsResult { + ensure!( + self.frame.ephem_origin_match(other.frame) + && self.frame.orient_origin_match(other.frame), + FrameMismatchSnafu { + action: "computing velocity RSS", + frame1: self.frame, + frame2: other.frame + } + ); + + Ok((self.radius_km - other.radius_km).norm()) + } + + /// Returns the absolute velocity difference in kilometer per second between this orbit and another. + /// Raises an error if the frames do not match (epochs do not need to match). + pub fn abs_vel_diff_km_s(&self, other: &Self) -> PhysicsResult { + ensure!( + self.frame.ephem_origin_match(other.frame) + && self.frame.orient_origin_match(other.frame), + FrameMismatchSnafu { + action: "computing velocity RSS", + frame1: self.frame, + frame2: other.frame + } + ); + + Ok((self.velocity_km_s - other.velocity_km_s).norm()) + } + + /// Returns the absolute position and velocity differences in km and km/s between this orbit and another. + /// Raises an error if the frames do not match (epochs do not need to match). + pub fn abs_difference(&self, other: &Self) -> PhysicsResult<(f64, f64)> { + Ok((self.abs_pos_diff_km(other)?, self.abs_vel_diff_km_s(other)?)) + } + + /// Returns the relative position difference (unitless) between this orbit and another. + /// This is computed by dividing the absolute difference by the norm of this object's radius vector. + /// If the radius is zero, this function raises a math error. + /// Raises an error if the frames do not match or (epochs do not need to match). + pub fn rel_pos_diff(&self, other: &Self) -> PhysicsResult { + if self.rmag_km() <= f64::EPSILON { + return Err(PhysicsError::AppliedMath { + source: MathError::DivisionByZero { + action: "computing relative position difference", + }, + }); + } + + Ok(self.abs_pos_diff_km(other)? / self.rmag_km()) + } + + /// Returns the absolute velocity difference in kilometer per second between this orbit and another. + /// Raises an error if the frames do not match (epochs do not need to match). + pub fn rel_vel_diff(&self, other: &Self) -> PhysicsResult { + if self.vmag_km_s() <= f64::EPSILON { + return Err(PhysicsError::AppliedMath { + source: MathError::DivisionByZero { + action: "computing relative velocity difference", + }, + }); + } + + Ok(self.abs_vel_diff_km_s(other)? / self.vmag_km_s()) + } + + /// Returns the relative difference between this orbit and another for the position and velocity, respectively the first and second return values. + /// Both return values are UNITLESS because the relative difference is computed as the absolute difference divided by the rmag and vmag of this object. + /// Raises an error if the frames do not match, if the position is zero or the velocity is zero. + pub fn rel_difference(&self, other: &Self) -> PhysicsResult<(f64, f64)> { + Ok((self.rel_pos_diff(other)?, self.rel_vel_diff(other)?)) + } } impl Add for CartesianState { diff --git a/anise/src/math/mod.rs b/anise/src/math/mod.rs index 9a86e2cb..e6645742 100644 --- a/anise/src/math/mod.rs +++ b/anise/src/math/mod.rs @@ -26,8 +26,8 @@ pub mod units; use nalgebra::allocator::Allocator; use nalgebra::{DefaultAllocator, DimName, OVector}; -/// Returns the root mean squared (RSS) between two vectors of any dimension N. -pub fn root_mean_squared(vec_a: &OVector, vec_b: &OVector) -> f64 +/// Returns the root sum squared (RSS) between two vectors of any dimension N. +pub fn root_sum_squared(vec_a: &OVector, vec_b: &OVector) -> f64 where DefaultAllocator: Allocator, { @@ -39,6 +39,21 @@ where .sqrt() } +/// Returns the root mean squared (RSS) between two vectors of any dimension N. +pub fn root_mean_squared(vec_a: &OVector, vec_b: &OVector) -> f64 +where + DefaultAllocator: Allocator, +{ + let sum_of_squares = vec_a + .iter() + .zip(vec_b.iter()) + .map(|(&x, &y)| (x - y).powi(2)) + .sum::(); + + let mean_of_squares = sum_of_squares / vec_a.len() as f64; + mean_of_squares.sqrt() +} + /// Returns the projection of a onto b /// Converted from NAIF SPICE's `projv` pub fn project_vector(a: &Vector3, b: &Vector3) -> Vector3 { diff --git a/anise/tests/astro/orbit.rs b/anise/tests/astro/orbit.rs index 222c0390..7c444846 100644 --- a/anise/tests/astro/orbit.rs +++ b/anise/tests/astro/orbit.rs @@ -4,6 +4,7 @@ use anise::astro::orbit::Orbit; use anise::constants::frames::EARTH_J2000; use anise::constants::usual_planetary_constants::MEAN_EARTH_ANGULAR_VELOCITY_DEG_S; use anise::math::angles::{between_0_360, between_pm_180}; +use anise::math::Vector3; use anise::prelude::*; use anise::time::{Epoch, Unit}; @@ -134,6 +135,10 @@ fn val_state_def_circ_inc(almanac: Almanac) { 8_191.929_999_991_808, "semi parameter" ); + let kep = Orbit::keplerian( + 8_191.93, 0.2, 12.85, 306.614, 314.19, -99.887_7, epoch, eme2k, + ); + f64_eq!(kep.ta_deg().unwrap(), 260.1123, "ta"); let ric_delta = kep.ric_difference(&kep).unwrap(); // Check that the frame is stripped @@ -151,13 +156,80 @@ fn val_state_def_circ_inc(almanac: Almanac) { f64_eq!( kep_rtn.rss_velocity_km_s(&kep).unwrap(), 0.0, - "RIC RSS velocity" + "RIC RMS velocity" + ); + f64_eq!(kep_rtn.rms_radius_km(&kep).unwrap(), 0.0, "RIC RSS radius"); + f64_eq!( + kep_rtn.rms_velocity_km_s(&kep).unwrap(), + 0.0, + "RIC RMS velocity" ); - let kep = Orbit::keplerian( - 8_191.93, 0.2, 12.85, 306.614, 314.19, -99.887_7, epoch, eme2k, + let vnc_delta = kep.vnc_difference(&kep).unwrap(); + // Check that the frame is stripped + assert!(vnc_delta.frame.mu_km3_s2.is_none()); + assert!(vnc_delta.frame.shape.is_none()); + // Check that the difference in radius magnitude and velocity are both zero + assert_eq!(vnc_delta.rmag_km(), 0.0); + assert_eq!(vnc_delta.vmag_km_s(), 0.0); + + // Check that the VNC computation is reciprocal. + let dcm = kep.dcm_from_ric_to_inertial().unwrap(); + let kep_ric = (dcm.transpose() * kep).unwrap(); + let kep_rtn = (dcm * kep_ric).unwrap(); + f64_eq!(kep_rtn.rss_radius_km(&kep).unwrap(), 0.0, "VNC RSS radius"); + f64_eq!( + kep_rtn.rss_velocity_km_s(&kep).unwrap(), + 0.0, + "VNC RMS velocity" + ); + f64_eq!(kep_rtn.rms_radius_km(&kep).unwrap(), 0.0, "VNC RSS radius"); + f64_eq!( + kep_rtn.rms_velocity_km_s(&kep).unwrap(), + 0.0, + "VNC RMS velocity" + ); + + // Check the relative and absolute differences + let (abs_pos_km, abs_vel_km_s) = kep_rtn.abs_difference(&kep).unwrap(); + f64_eq!( + abs_pos_km.abs(), + 0.0, + "non zero absolute position difference" + ); + f64_eq!( + abs_vel_km_s.abs(), + 0.0, + "non zero absolute velocity difference" + ); + + let (rel_pos, rel_vel) = kep_rtn.rel_difference(&kep).unwrap(); + f64_eq!(rel_pos.abs(), 0.0, "non zero relative position difference"); + f64_eq!(rel_vel.abs(), 0.0, "non zero relative velocity difference"); + + // Ensure that setting the radius or velocity to zero causes an error in relative difference + let mut zero_pos = kep; + zero_pos.radius_km = Vector3::zeros(); + assert!( + zero_pos.rel_pos_diff(&kep).is_err(), + "radius of self must be non zero" + ); + assert!( + kep.rel_pos_diff(&zero_pos).is_ok(), + "only self needs a non zero radius" + ); + + // Ensure that setting the radius or velocity to zero causes an error in relative difference + let mut zero_vel = kep; + zero_vel.velocity_km_s = Vector3::zeros(); + assert!( + zero_vel.rel_vel_diff(&kep).is_err(), + "velocity of self must be non zero" + ); + assert!( + kep.rel_vel_diff(&zero_vel).is_ok(), + "only self needs a non zero velocity" ); - f64_eq!(kep.ta_deg().unwrap(), 260.1123, "ta"); } #[rstest] From b60af8931e523ae64dafd450a3e6b0ecfd1f488d Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Fri, 13 Sep 2024 16:19:58 -0600 Subject: [PATCH 2/2] Update arrow and parquet to 53.x --- anise/Cargo.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/anise/Cargo.toml b/anise/Cargo.toml index 26b83ef9..bf33b656 100644 --- a/anise/Cargo.toml +++ b/anise/Cargo.toml @@ -45,8 +45,8 @@ regex = { version = "1.10.5", optional = true } [dev-dependencies] rust-spice = "0.7.6" -parquet = "52.0.0" -arrow = "52.0.0" +parquet = "53.0.0" +arrow = "53.0.0" criterion = "0.5" iai = "0.1" polars = { version = "0.42.0", features = ["lazy", "parquet"] }