diff --git a/Cargo.lock b/Cargo.lock index 0376afd0a..40d821781 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1595,6 +1595,10 @@ version = "1.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6ce2be8dc25455e1f91df71bfa12ad37d7af1092ae736f3a6cd0e37bc7810596" +[[package]] +name = "statime-algo" +version = "2.0.0-alpha.20260715" + [[package]] name = "statime-csptp" version = "2.0.0-alpha.20260715" diff --git a/Cargo.toml b/Cargo.toml index 19af87fde..ca39883db 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -2,7 +2,7 @@ members = [ "ntp-proto", "ntpd" -, "statime-csptp", "statime-wire", "statime-netptp"] +, "statime-algo", "statime-csptp", "statime-wire", "statime-netptp"] exclude = [ ] # Properly take compiler version into account when resolving crates. diff --git a/statime-algo/Cargo.toml b/statime-algo/Cargo.toml new file mode 100644 index 000000000..46423f20d --- /dev/null +++ b/statime-algo/Cargo.toml @@ -0,0 +1,20 @@ +[package] +name = "statime-algo" +version.workspace = true +edition.workspace = true +license.workspace = true +repository.workspace = true +homepage.workspace = true +readme.workspace = true +description.workspace = true +publish.workspace = true +rust-version.workspace = true + +[lints] +workspace = true + +[dependencies] + +[features] +default = ["std"] +std = [] diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs new file mode 100644 index 000000000..08b52076b --- /dev/null +++ b/statime-algo/src/estimator.rs @@ -0,0 +1,988 @@ +use std::{boxed::Box, vec::Vec}; + +use crate::matrix::{Matrix, MatrixError}; + +use super::{ClockId, LinkId}; + +//FIXME: Replace with proper Timestamp type +type Timestamp = f64; + +//FIXME: Make more permanent error enum +/// Errors that can occur when using the estimator. +#[derive(Debug, Clone, PartialEq, Eq, Hash)] +pub enum EstimatorError { + /// Clock was not found in the state + ClockNotFound, + /// Clock already exists in the state + ClockAlreadyExists, + /// Link not found in the state + LinkNotFound, + /// Link already exists in the state + LinkAlreadyExists, + /// Measurement between two external clocks is not allowed + MeasurementBetweenExternalClocks, + /// Measurement between a clock with itself is not allowed + MeasurementBetweenSelf, + /// Time moved backwards, which is not allowed + NonMonotonicTimeProgression, + /// Error from the underlying matrix library + MatrixError(MatrixError), +} + +impl From for EstimatorError { + fn from(err: MatrixError) -> Self { + EstimatorError::MatrixError(err) + } +} + +#[derive(Debug, Clone)] +struct ExternalClockList(Vec); + +impl ExternalClockList { + fn new() -> ExternalClockList { + ExternalClockList(Vec::new()) + } + + /// Returns true if the given clock is known as an external clock. + fn contains(&self, id: ClockId) -> bool { + self.0.contains(&id) + } + + /// Add a new external clock to the list. + fn add(&mut self, id: ClockId) -> Result<(), EstimatorError> { + if self.contains(id) { + Err(EstimatorError::ClockAlreadyExists) + } else { + self.0.push(id); + Ok(()) + } + } + + /// Remove an existing external clock from the list. + fn remove(&mut self, id: ClockId) -> Result<(), EstimatorError> { + if let Some(pos) = self.0.iter().position(|&x| x == id) { + self.0.remove(pos); + Ok(()) + } else { + Err(EstimatorError::ClockNotFound) + } + } +} + +#[derive(Debug, Clone, Copy)] +struct ClockInfo { + id: ClockId, + base_index: usize, + wander: f64, +} + +impl ClockInfo { + const SIZE: usize = 2; + + fn offset_index(self) -> usize { + self.base_index + } + + fn frequency_index(self) -> usize { + self.base_index + 1 + } +} + +#[derive(Debug, Clone)] +struct ClockInfoList(Vec); + +impl ClockInfoList { + fn new() -> ClockInfoList { + ClockInfoList(Vec::new()) + } + + /// Checks if the given clock id exists in the current list. + fn contains(&self, id: ClockId) -> bool { + self.0.iter().any(|info| info.id == id) + } + + /// Update the base indices of all clocks that have a base index greater + /// than `from`, by subtracting `delta` from them. + fn update_indices(&mut self, from: usize, delta: usize) { + for info in &mut self.0 { + if info.base_index > from { + info.base_index -= delta; + } + } + } + + /// Remove the clock info for a given id, if it exists. + /// + /// Updates the indices for clocks and links where needed. + /// + /// Returns the removed clock info. + fn remove( + &mut self, + id: ClockId, + link_info: &mut LinkInfoList, + ) -> Result { + let removed = if let Some(pos) = self.0.iter().position(|info| info.id == id) { + Ok(self.0.remove(pos)) + } else { + Err(EstimatorError::ClockNotFound) + }?; + + self.update_indices(removed.base_index, ClockInfo::SIZE); + link_info.update_indices(removed.base_index, ClockInfo::SIZE); + + Ok(removed) + } + + /// Add a new clock info to the list, if it doesn't already exist. + fn add(&mut self, info: ClockInfo) -> Result<(), EstimatorError> { + if self.0.iter().any(|existing| existing.id == info.id) { + Err(EstimatorError::ClockAlreadyExists) + } else { + self.0.push(info); + Ok(()) + } + } + + /// Iterate over all clocks in the list. + fn iter(&self) -> impl Iterator { + self.0.iter() + } +} + +#[derive(Debug, Clone, Copy)] +struct LinkInfo { + id: LinkId, + index: usize, + /// Fraction of the link delay that we assume the error increases by every second + decay_rate: f64, +} + +impl LinkInfo { + const SIZE: usize = 1; +} + +#[derive(Debug, Clone)] +struct LinkInfoList(Vec); + +impl LinkInfoList { + fn new() -> LinkInfoList { + LinkInfoList(Vec::new()) + } + + /// Update the indices of all links that have an index greater than `from`, by subtracting `delta` from them. + fn update_indices(&mut self, from: usize, delta: usize) { + for info in &mut self.0 { + if info.index > from { + info.index -= delta; + } + } + } + + /// Remove the link info for a given id, if it exists. + /// + /// Updates the indices for links and clocks where needed. + /// + /// Returns the removed link info. + fn remove( + &mut self, + id: LinkId, + clock_info: &mut ClockInfoList, + ) -> Result { + let removed = if let Some(pos) = self.0.iter().position(|info| info.id == id) { + Ok(self.0.remove(pos)) + } else { + Err(EstimatorError::LinkNotFound) + }?; + + self.update_indices(removed.index, LinkInfo::SIZE); + clock_info.update_indices(removed.index, LinkInfo::SIZE); + + Ok(removed) + } + + /// Add a new link info to the list, if it doesn't already exist. + fn add(&mut self, info: LinkInfo) -> Result<(), EstimatorError> { + if self.0.iter().any(|existing| existing.id == info.id) { + Err(EstimatorError::LinkAlreadyExists) + } else { + self.0.push(info); + Ok(()) + } + } + + /// Iterate over all links in the list. + fn iter(&self) -> impl Iterator { + self.0.iter() + } +} + +/// Represents the state of the estimator at a given point in time. +/// +/// Note how mutating methods on this all consume self. This is on purpose, +/// as it makes it easier to reason about the state of the estimator. This +/// does however mean that errors result in the state being lost. In general +/// it is expected that most errors are unrecoverable. Typically one would +/// keep a list of some of the most recent states though, they could be used +/// for error recovery, but also for tracability and debugging. +#[derive(Debug, Clone)] +pub struct EstimatorState { + time: Timestamp, + state: Matrix>, + uncertainty: Matrix>, + clock_info: ClockInfoList, + external_clocks: ExternalClockList, + link_info: LinkInfoList, +} + +/// Represents an uncertain value, with a best estimate and an uncertainty (standard deviation). +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct UncertainValue { + /// Best estimate of the value + pub value: f64, + /// Square root of the variance of the value. Corresponds + /// to 1 standard deviation. + pub uncertainty: f64, +} + +/// Convert from a tuple of (value, uncertainty) to an `UncertainValue`. +impl From<(f64, f64)> for UncertainValue { + fn from(value: (f64, f64)) -> Self { + UncertainValue { + value: value.0, + uncertainty: value.1, + } + } +} + +impl EstimatorState { + /// Create a new empty estimator state at the given timestamp. + /// + /// This state has no clocks or links contained in it. + #[must_use] + pub fn empty(time: Timestamp) -> EstimatorState { + EstimatorState { + time, + state: Matrix::zero(0, 1), + uncertainty: Matrix::zero(0, 0), + clock_info: ClockInfoList::new(), + external_clocks: ExternalClockList::new(), + link_info: LinkInfoList::new(), + } + } + + /// Progress the estimator state to the new timestamp. + pub fn progress_time(mut self, new_time: Timestamp) -> Result { + // time should not move backwards + if new_time < self.time { + return Err(EstimatorError::NonMonotonicTimeProgression); + } + + // no time change, return state as is + #[expect(clippy::float_cmp, reason = "Explicit delta is zero short circuit")] + if new_time == self.time { + return Ok(self); + } + + let delta_t = new_time - self.time; + + let mut update = Matrix::identity(self.state.rows()); + let mut noise = Matrix::zero(self.state.rows(), self.state.rows()); + + // For each clock, we need to determine a value for the update and noise matrices. + for clock_info in self.clock_info.iter() { + update[(clock_info.offset_index(), clock_info.frequency_index())] = delta_t; + // We need to square wander as we store it in units of ppm per second, + // which is a standard deviation (effectively). + // The powers of time that contribute to clock process noise can be + // derived from imposing the random-walk requirement on the frequency, + // which gives linear relationship for the frequency variance increase, + // and then requiring that two updates for shorter intervals give the same + // result as one update for the sum of those intervals. + noise[(clock_info.offset_index(), clock_info.offset_index())] = + delta_t.powi(3) * clock_info.wander.powi(2) / 3.; + noise[(clock_info.offset_index(), clock_info.frequency_index())] = + delta_t.powi(2) * clock_info.wander.powi(2) / 2.; + noise[(clock_info.frequency_index(), clock_info.offset_index())] = + delta_t.powi(2) * clock_info.wander.powi(2) / 2.; + noise[(clock_info.frequency_index(), clock_info.frequency_index())] = + delta_t * clock_info.wander.powi(2); + } + + for link_info in self.link_info.iter() { + noise[(link_info.index, link_info.index)] = + delta_t * ((link_info.decay_rate * self.state[(link_info.index, 0)]).powi(2)); + } + + self.time = new_time; + self.state = &update * &self.state; + self.uncertainty = &update * &self.uncertainty * update.transpose() + noise; + + Ok(self) + } + + /// Absorb a change in frequency of a clock. + /// + /// This function assumes steering happens at the current filter time. + pub fn absorb_frequency_steer( + mut self, + steered_clock: ClockId, + frequency_change: f64, + ) -> Result { + let clock_info = self.get_clock_info(steered_clock)?; + let frequency_index = clock_info.frequency_index(); + self.state[(frequency_index, 0)] += frequency_change; + Ok(self) + } + + /// Add a new measurement to the estimator state. + /// + /// Assumes the measurements happens at the time the estimator state is + /// currently set to. + pub fn measurement( + mut self, + from: ClockId, + to: ClockId, + offset: UncertainValue, + link_delay: Option, + ) -> Result { + if from == to { + return Err(EstimatorError::MeasurementBetweenSelf); + } + + let mut measurement_projection = Matrix::zero(1, self.state.rows()); + + let from_external = self.external_clocks.contains(from); + let to_external = self.external_clocks.contains(to); + + if from_external && to_external { + return Err(EstimatorError::MeasurementBetweenExternalClocks); + } + + if !from_external { + let from_clock_info = self.get_clock_info(from)?; + measurement_projection[(0, from_clock_info.offset_index())] = -1.0; + } + + if !to_external { + let to_clock_info = self.get_clock_info(to)?; + measurement_projection[(0, to_clock_info.offset_index())] = 1.0; + } + + if let Some(link_delay) = link_delay { + let link_delay_info = self.get_link_info(link_delay)?; + measurement_projection[(0, link_delay_info.index)] = 1.0; + } + + let expected = &measurement_projection * &self.state; + let difference = Matrix::>::from(offset.value) - expected; + // The uncertainty of the difference between measurement and prediction is the sum of + // the uncertainty of the measurement, and the uncertainty on the prediction. The + // prediction uncertainty can be shown to follow from multiplying the state uncertainty + // from both sides by the measurement projection. Intuitively this is because the + // uncertainty is sort of a square of the state. + let difference_covariance = + &measurement_projection * &self.uncertainty * measurement_projection.transpose() + + Matrix::from(offset.uncertainty.powi(2)); + + // Intuitively, the multiplication with the measurement gives the contribution + // for each part of the state to the uncertainty of the measurement prediction. + // The division then normalizes that to weights on how large the change to each + // part of the state needs to be. This makes sense because where our prediction + // has more uncertainty from, the measurement should weigh more. + let update_strength = + &self.uncertainty * measurement_projection.transpose() / difference_covariance[(0, 0)]; + + // This is simply using the strength we calculated before to update the state + self.state = &self.state + &update_strength * difference; + + // However I don't have a good intuition why this would be its uncertainty. It + // is derived well on wikipedia, and when having questions I would suggest looking + // at its page on kalman filters. + let prev_step_proportionality = + Matrix::identity(self.state.rows()) - &update_strength * measurement_projection; + self.uncertainty = (&prev_step_proportionality + * &self.uncertainty + * prev_step_proportionality.transpose() + + &update_strength * offset.uncertainty.powi(2) * update_strength.transpose()) + .symmetrize()?; + + Ok(self) + } + + /// Add an external clock to the estimator state. + pub fn add_external_clock(mut self, id: ClockId) -> Result { + // check in clock info as well + if self.clock_info.contains(id) { + return Err(EstimatorError::ClockAlreadyExists); + } + + self.external_clocks.add(id)?; + + Ok(self) + } + + /// Remove an external clock from the estimator state. + pub fn remove_external_clock(mut self, id: ClockId) -> Result { + self.external_clocks.remove(id)?; + + Ok(self) + } + + /// Add a new clock to the estimator state.' + /// + /// To add a new clock you must provide the initial values for the offset, + /// frequency and wander of the clock. + pub fn add_clock( + mut self, + id: ClockId, + initial_offset: UncertainValue, + initial_frequency: UncertainValue, + initial_wander: f64, + ) -> Result { + // check in external clocks as well + if self.external_clocks.contains(id) { + return Err(EstimatorError::ClockAlreadyExists); + } + + let new_clock_info = ClockInfo { + id, + base_index: self.state.rows(), + wander: initial_wander, + }; + + self.clock_info.add(new_clock_info)?; + self.state = self + .state + .extend_vec([initial_offset.value, initial_frequency.value])?; + self.uncertainty = self.uncertainty.extend([ + [initial_offset.uncertainty.powi(2), 0.0], + [0.0, initial_frequency.uncertainty.powi(2)], + ]); + + Ok(self) + } + + /// Remove a clock from the estimator state. + pub fn remove_clock(mut self, id: ClockId) -> Result { + let clock_info = self.clock_info.remove(id, &mut self.link_info)?; + + self.state = self + .state + .splice_vec(clock_info.base_index, ClockInfo::SIZE)?; + self.uncertainty = self + .uncertainty + .splice_square(clock_info.base_index, ClockInfo::SIZE)?; + + Ok(self) + } + + /// Add a new link to the estimator state. + /// + /// The decay rate is the amount the uncertainty on the link delay increases every second on this link. + pub fn add_link( + mut self, + id: LinkId, + initial_delay: UncertainValue, + decay_rate: f64, + ) -> Result { + let new_link_info = LinkInfo { + id, + index: self.state.rows(), + decay_rate, + }; + + self.link_info.add(new_link_info)?; + self.state = self.state.extend_vec([initial_delay.value])?; + self.uncertainty = self + .uncertainty + .extend([[initial_delay.uncertainty.powi(2)]]); + + Ok(self) + } + + /// Remove a link from the estimator state. + pub fn remove_link(mut self, id: LinkId) -> Result { + let removed_info = self.link_info.remove(id, &mut self.clock_info)?; + self.state = self.state.splice_vec(removed_info.index, LinkInfo::SIZE)?; + self.uncertainty = self + .uncertainty + .splice_square(removed_info.index, LinkInfo::SIZE)?; + + Ok(self) + } + + /// Get the current offset of a clock in the state, along with the uncertainty of that offset. + pub fn clock_offset(&self, id: ClockId) -> Result { + let clock_info = self.get_clock_info(id)?; + Ok(UncertainValue { + value: self.state[(clock_info.offset_index(), 0)], + uncertainty: self.uncertainty[(clock_info.offset_index(), clock_info.offset_index())] + .sqrt(), + }) + } + + /// Get the current frequency of a clock in the state, along with the uncertainty of that frequency. + pub fn clock_frequency(&self, id: ClockId) -> Result { + let clock_info = self.get_clock_info(id)?; + Ok(UncertainValue { + value: self.state[(clock_info.frequency_index(), 0)], + uncertainty: self.uncertainty + [(clock_info.frequency_index(), clock_info.frequency_index())] + .sqrt(), + }) + } + + /// Get the current delay of a link in the state, along with the uncertainty of that delay. + pub fn link_delay(&self, id: LinkId) -> Result { + let link_info = self.get_link_info(id)?; + Ok(UncertainValue { + value: self.state[(link_info.index, 0)], + uncertainty: self.uncertainty[(link_info.index, link_info.index)].sqrt(), + }) + } + + /// Is a given clock internal + pub fn is_internal_clock(&self, id: ClockId) -> bool { + self.clock_info.iter().any(|info| info.id == id) + } + + /// Is a given clock external + pub fn is_external_clock(&self, id: ClockId) -> bool { + self.external_clocks.contains(id) + } +} + +impl EstimatorState { + fn get_clock_info(&self, id: ClockId) -> Result<&ClockInfo, EstimatorError> { + self.clock_info + .iter() + .find(|info| info.id == id) + .ok_or(EstimatorError::ClockNotFound) + } + + fn get_link_info(&self, id: LinkId) -> Result<&LinkInfo, EstimatorError> { + self.link_info + .iter() + .find(|info| info.id == id) + .ok_or(EstimatorError::LinkNotFound) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + macro_rules! assert_almost_eq { + ($left:expr, $right:expr) => { + match (&$left, &$right) { + (left_val, right_val) => { + assert!((*left_val - *right_val).abs() <= 1e-6*right_val.abs(), + "Floating point values not almost equal.\nLeft={left_val}\nRight={right_val}") + } + } + }; + } + + macro_rules! assert_uv_almost_eq { + ($left:expr, $right:expr) => { + match (&$left, &$right) { + (left_val, right_val) => { + assert!((left_val.value - right_val.value).abs() <= 1e-6*right_val.value.abs(), + "Floating point values not almost equal.\nLeft={left_val:?}\nRight={right_val:?}"); + assert!((left_val.uncertainty - right_val.uncertainty).abs() <= 1e-6*right_val.uncertainty.abs(), + "Floating point uncertainty not almost equal.\nLeft={left_val:?}\nRight={right_val:?}"); + } + } + }; + } + + #[test] + fn test_add_clock() { + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 1.0).into(), (2.0, 3.0).into(), 1e-8) + .unwrap(); + assert_eq!(state.clock_offset(ClockId(1)).unwrap().value, 0.0); + assert_eq!(state.clock_offset(ClockId(1)).unwrap().uncertainty, 1.0); + assert_eq!(state.clock_frequency(ClockId(1)).unwrap().value, 2.0); + assert_eq!(state.clock_frequency(ClockId(1)).unwrap().uncertainty, 3.0); + + assert_eq!( + state.clone().add_external_clock(ClockId(1)).unwrap_err(), + EstimatorError::ClockAlreadyExists + ); + let state = state.add_external_clock(ClockId(2)).unwrap(); + + assert_eq!( + state + .add_clock(ClockId(2), (0.0, 1.0).into(), (2.0, 3.0).into(), 1e-8) + .unwrap_err(), + EstimatorError::ClockAlreadyExists + ); + } + + #[test] + fn test_clock_removal() { + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 1.0).into(), (0.0, 1.0).into(), 1e-8) + .unwrap() + .add_external_clock(ClockId(2)) + .unwrap(); + + // remove non-existing clock should fail + assert_eq!( + state.clone().remove_clock(ClockId(3)).unwrap_err(), + EstimatorError::ClockNotFound + ); + + // remove existing clock via external clock removal should fail + assert_eq!( + state.clone().remove_external_clock(ClockId(1)).unwrap_err(), + EstimatorError::ClockNotFound + ); + + // remove existing external clock via internal clock removal should fail + assert_eq!( + state.clone().remove_clock(ClockId(2)).unwrap_err(), + EstimatorError::ClockNotFound + ); + + // removing the existing clocks should succeed + state + .remove_clock(ClockId(1)) + .unwrap() + .remove_external_clock(ClockId(2)) + .unwrap(); + } + + #[test] + fn test_time_evolve() { + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 0.0).into(), (1e-6, 0.0).into(), 1e-8) + .unwrap() + .add_link(LinkId(1), (0.5, 0.2).into(), 0.0) + .unwrap() + .add_clock(ClockId(2), (0.0, 1e-5).into(), (-1e-6, 1e-7).into(), 0.0) + .unwrap() + .add_link(LinkId(2), (2.0, 0.0).into(), 0.1) + .unwrap() + .progress_time(100.0) + .unwrap(); + assert_eq!(state.clock_frequency(ClockId(1)).unwrap().value, 1e-6); + // Random walk noise, so frequency deviation is sqrt(time_interval)*wander. + assert_almost_eq!(state.clock_frequency(ClockId(1)).unwrap().uncertainty, 1e-7); + // Pre-existing frequency offset should cause phase offset. + assert_almost_eq!(state.clock_offset(ClockId(1)).unwrap().value, 1e-4); + // Random walk noise in the derivative, so the integral gives an + // additional factor of time compared to the frequency deviation. + // The factor sqrt(3) follows from the structure of how updates work. + assert_almost_eq!( + state.clock_offset(ClockId(1)).unwrap().uncertainty, + 1e-5 / (3.0f64.sqrt()) + ); + + let state = state.remove_clock(ClockId(1)).unwrap(); + + assert_eq!(state.link_delay(LinkId(1)).unwrap().value, 0.5); + assert_eq!(state.link_delay(LinkId(1)).unwrap().uncertainty, 0.2); + + let state = state.remove_link(LinkId(1)).unwrap(); + + assert_eq!(state.clock_frequency(ClockId(2)).unwrap().value, -1e-6); + assert_eq!(state.clock_frequency(ClockId(2)).unwrap().uncertainty, 1e-7); + assert_almost_eq!(state.clock_offset(ClockId(2)).unwrap().value, -1e-4); + assert_almost_eq!( + state.clock_offset(ClockId(2)).unwrap().uncertainty, + 1e-5 * (2.0f64.sqrt()) + ); + + assert_uv_almost_eq!( + state.link_delay(LinkId(2)).unwrap(), + UncertainValue::from((2.0, 2.0)) + ); + } + + #[test] + fn test_frequency_steering() { + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 0.0).into(), (1e-6, 0.0).into(), 1e-8) + .unwrap() + .absorb_frequency_steer(ClockId(1), -1e-6) + .unwrap(); + + assert_uv_almost_eq!( + state.clock_frequency(ClockId(1)).unwrap(), + UncertainValue::from((0.0, 0.0)) + ); + } + + #[test] + fn test_progress_time_composes_well() { + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 0.0).into(), (1e-6, 0.0).into(), 1e-8) + .unwrap(); + + let state_via_intermediate = state + .clone() + .progress_time(75.0) + .unwrap() + .progress_time(100.0) + .unwrap(); + let state_at_once = state.progress_time(100.0).unwrap(); + + assert_uv_almost_eq!( + state_at_once.clock_offset(ClockId(1)).unwrap(), + state_via_intermediate.clock_offset(ClockId(1)).unwrap() + ); + assert_uv_almost_eq!( + state_at_once.clock_frequency(ClockId(1)).unwrap(), + state_via_intermediate.clock_frequency(ClockId(1)).unwrap() + ); + } + + #[test] + fn test_add_link() { + let state = EstimatorState::empty(0.0) + .add_link(LinkId(1), (1.0, 2.0).into(), 0.0) + .expect("Failed to add link"); + assert_eq!(state.link_delay(LinkId(1)).unwrap().value, 1.0); + assert_eq!(state.link_delay(LinkId(1)).unwrap().uncertainty, 2.0); + } + + #[test] + fn test_measure_between_clocks_no_link() { + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap() + .add_clock(ClockId(2), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap() + .measurement( + ClockId(1), + ClockId(2), + (1.0, 2.0f64.sqrt() * 0.1).into(), + None, + ) + .unwrap(); + + assert_uv_almost_eq!( + state.clock_offset(ClockId(1)).unwrap(), + UncertainValue::from((-0.25, 0.05 * (3.0f64.sqrt()))) + ); + assert_uv_almost_eq!( + state.clock_offset(ClockId(2)).unwrap(), + UncertainValue::from((0.25, 0.05 * (3.0f64.sqrt()))) + ); + assert_uv_almost_eq!( + state.clock_frequency(ClockId(1)).unwrap(), + UncertainValue::from((0.0, 1e-8)) + ); + assert_uv_almost_eq!( + state.clock_frequency(ClockId(2)).unwrap(), + UncertainValue::from((0.0, 1e-8)) + ); + + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 0.0).into(), (0.0, 1e-3).into(), 0.0) + .unwrap() + .add_clock(ClockId(2), (0.0, 0.0).into(), (0.0, 1e-3).into(), 0.0) + .unwrap() + .progress_time(100.0) + .unwrap() + .measurement( + ClockId(1), + ClockId(2), + (1.0, 2.0f64.sqrt() * 0.1).into(), + None, + ) + .unwrap(); + + assert_uv_almost_eq!( + state.clock_offset(ClockId(1)).unwrap(), + UncertainValue::from((-0.25, 0.05 * (3.0f64.sqrt()))) + ); + assert_uv_almost_eq!( + state.clock_offset(ClockId(2)).unwrap(), + UncertainValue::from((0.25, 0.05 * (3.0f64.sqrt()))) + ); + assert_uv_almost_eq!( + state.clock_frequency(ClockId(1)).unwrap(), + UncertainValue::from((-0.0025, 0.0005 * (3.0f64.sqrt()))) + ); + assert_uv_almost_eq!( + state.clock_frequency(ClockId(2)).unwrap(), + UncertainValue::from((0.0025, 0.0005 * (3.0f64.sqrt()))) + ); + } + + #[test] + fn test_measure_between_clocks_with_link() { + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap() + .add_clock(ClockId(2), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap() + .add_link(LinkId(1), (1.0, 0.0).into(), 0.0) + .unwrap() + .measurement( + ClockId(1), + ClockId(2), + (2.0, 2.0f64.sqrt() * 0.1).into(), + Some(LinkId(1)), + ) + .unwrap(); + + assert_uv_almost_eq!( + state.clock_offset(ClockId(1)).unwrap(), + UncertainValue::from((-0.25, 0.05 * (3.0f64.sqrt()))) + ); + assert_uv_almost_eq!( + state.clock_offset(ClockId(2)).unwrap(), + UncertainValue::from((0.25, 0.05 * (3.0f64.sqrt()))) + ); + assert_uv_almost_eq!( + state.clock_frequency(ClockId(1)).unwrap(), + UncertainValue::from((0.0, 1e-8)) + ); + assert_uv_almost_eq!( + state.clock_frequency(ClockId(2)).unwrap(), + UncertainValue::from((0.0, 1e-8)) + ); + assert_uv_almost_eq!( + state.link_delay(LinkId(1)).unwrap(), + UncertainValue::from((1.0, 0.0)) + ); + + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 0.0).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap() + .add_clock(ClockId(2), (0.0, 0.0).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap() + .add_link(LinkId(1), (0.0, 0.1).into(), 0.0) + .unwrap() + .measurement(ClockId(1), ClockId(2), (1.0, 0.1).into(), Some(LinkId(1))) + .unwrap(); + + assert_uv_almost_eq!( + state.clock_offset(ClockId(1)).unwrap(), + UncertainValue::from((0.0, 0.0)) + ); + assert_uv_almost_eq!( + state.clock_offset(ClockId(2)).unwrap(), + UncertainValue::from((0.0, 0.0)) + ); + assert_uv_almost_eq!( + state.clock_frequency(ClockId(1)).unwrap(), + UncertainValue::from((0.0, 1e-8)) + ); + assert_uv_almost_eq!( + state.clock_frequency(ClockId(2)).unwrap(), + UncertainValue::from((0.0, 1e-8)) + ); + assert_uv_almost_eq!( + state.link_delay(LinkId(1)).unwrap(), + UncertainValue::from((0.5, 0.1 / (2.0f64.sqrt()))) + ); + } + + #[test] + fn test_measure_external_clock_no_link() { + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap() + .add_external_clock(ClockId(2)) + .unwrap() + .measurement(ClockId(2), ClockId(1), (1.0, 0.1).into(), None) + .unwrap(); + + assert_uv_almost_eq!( + state.clock_offset(ClockId(1)).unwrap(), + UncertainValue::from((0.5, 0.1 / (2.0f64.sqrt()))) + ); + + assert_uv_almost_eq!( + state.clock_frequency(ClockId(1)).unwrap(), + UncertainValue::from((0.0, 1e-8)) + ); + + let state = EstimatorState::empty(0.0) + .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap() + .add_external_clock(ClockId(2)) + .unwrap() + .measurement(ClockId(1), ClockId(2), (1.0, 0.1).into(), None) + .unwrap(); + + assert_uv_almost_eq!( + state.clock_offset(ClockId(1)).unwrap(), + UncertainValue::from((-0.5, 0.1 / (2.0f64.sqrt()))) + ); + + assert_uv_almost_eq!( + state.clock_frequency(ClockId(1)).unwrap(), + UncertainValue::from((0.0, 1e-8)) + ); + + assert!(state.remove_external_clock(ClockId(2)).is_ok()); + } + + #[test] + fn test_negative_time_step() { + let state = EstimatorState::empty(0.0); + assert_eq!( + state.clone().progress_time(-1.0).unwrap_err(), + EstimatorError::NonMonotonicTimeProgression + ); + } + + #[test] + fn test_invalid_measurements() { + let state = EstimatorState::empty(0.0) + .add_external_clock(ClockId(1)) + .unwrap() + .add_external_clock(ClockId(2)) + .unwrap() + .add_clock(ClockId(3), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap() + .add_clock(ClockId(4), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap(); + + assert_eq!( + state + .clone() + .measurement(ClockId(1), ClockId(2), (0.0, 0.1).into(), None) + .unwrap_err(), + EstimatorError::MeasurementBetweenExternalClocks + ); + + assert_eq!( + state + .clone() + .measurement(ClockId(3), ClockId(5), (0.0, 0.1).into(), None) + .unwrap_err(), + EstimatorError::ClockNotFound + ); + + assert_eq!( + state + .clone() + .measurement(ClockId(5), ClockId(3), (0.0, 0.1).into(), None) + .unwrap_err(), + EstimatorError::ClockNotFound + ); + + assert_eq!( + state + .clone() + .measurement(ClockId(3), ClockId(4), (0.0, 0.1).into(), Some(LinkId(1))) + .unwrap_err(), + EstimatorError::LinkNotFound + ); + + assert_eq!( + state + .clone() + .measurement(ClockId(3), ClockId(3), (0.0, 0.1).into(), None) + .unwrap_err(), + EstimatorError::MeasurementBetweenSelf + ); + } +} diff --git a/statime-algo/src/filter.rs b/statime-algo/src/filter.rs new file mode 100644 index 000000000..0e44ff410 --- /dev/null +++ b/statime-algo/src/filter.rs @@ -0,0 +1,198 @@ +use crate::{ + ClockId, EstimatorError, EstimatorState, LinkId, LinkNoiseError, LinkNoiseEstimator, + estimator::UncertainValue, ringbuffer::UnorderedRingBuffer, +}; + +type Timestamp = f64; + +#[derive(Debug, Clone, PartialEq, Eq, Hash)] +pub enum LinkFilterError { + UnknownClock, + UnknownLink, + BothClocksExternal, + ClocksEqual, + EstimatorError(EstimatorError), + LinkNoiseError(LinkNoiseError), +} + +impl From for LinkFilterError { + fn from(value: EstimatorError) -> Self { + LinkFilterError::EstimatorError(value) + } +} + +impl From for LinkFilterError { + fn from(value: LinkNoiseError) -> Self { + LinkFilterError::LinkNoiseError(value) + } +} + +enum LinkState { + Tracked(LinkNoiseEstimator), + Untracked(ClockId, ClockId), +} + +struct LinkInfo { + id: LinkId, + active: bool, + internal: bool, + link_state: LinkState, + last_offsets: UnorderedRingBuffer, +} + +pub struct LinkFilter { + links: std::vec::Vec, + estimation_state: EstimatorState, +} + +impl LinkFilter { + pub fn empty(time: Timestamp) -> Self { + LinkFilter { + links: std::vec::Vec::new(), + estimation_state: EstimatorState::empty(time), + } + } + + pub fn progress_time(mut self, new_time: Timestamp) -> Result { + self.estimation_state = self.estimation_state.progress_time(new_time)?; + Ok(self) + } + + pub fn absorb_frequency_steer( + mut self, + steered_clock: ClockId, + frequency_change: f64, + ) -> Result { + self.estimation_state = self + .estimation_state + .absorb_frequency_steer(steered_clock, frequency_change)?; + Ok(self) + } + + pub fn measurement( + self, + from: ClockId, + to: ClockId, + offset: UncertainValue, + link: LinkId, + ) -> Result { + todo!() + } + + pub fn add_external_clock(mut self) -> Result<(Self, ClockId), LinkFilterError> { + let id = ClockId::new(); + self.estimation_state = self.estimation_state.add_external_clock(id)?; + Ok((self, id)) + } + + pub fn remove_external_clock(mut self, id: ClockId) -> Result { + // FIXME: check for existence of links with this clock + self.estimation_state = self.estimation_state.remove_external_clock(id)?; + Ok(self) + } + + pub fn add_clock( + mut self, + initial_offset: UncertainValue, + initial_frequency: UncertainValue, + initial_wander: f64, + ) -> Result<(Self, ClockId), LinkFilterError> { + let id = ClockId::new(); + self.estimation_state = self.estimation_state.add_clock( + id, + initial_offset, + initial_frequency, + initial_wander, + )?; + Ok((self, id)) + } + + pub fn remove_clock(mut self, id: ClockId) -> Result { + // FIXME: check for existence of links with this clock + self.estimation_state = self.estimation_state.remove_clock(id)?; + Ok(self) + } + + pub fn add_tracked_link( + mut self, + clock_a: ClockId, + clock_b: ClockId, + ) -> Result<(Self, LinkId), LinkFilterError> { + if clock_a == clock_b { + return Err(LinkFilterError::ClocksEqual); + } + + let clock_a_internal = self.estimation_state.is_internal_clock(clock_a); + let clock_a_external = self.estimation_state.is_external_clock(clock_a); + let clock_b_internal = self.estimation_state.is_internal_clock(clock_b); + let clock_b_external = self.estimation_state.is_external_clock(clock_b); + + if !(clock_a_internal || clock_a_external) || !(clock_b_internal || clock_b_external) { + return Err(LinkFilterError::UnknownClock); + } + + if clock_a_external && clock_b_external { + return Err(LinkFilterError::BothClocksExternal); + } + + let id = LinkId::new(); + let is_internal = clock_a_internal && clock_b_internal; + self.links.push(LinkInfo { + id, + active: false, + internal: is_internal, + link_state: LinkState::Tracked(LinkNoiseEstimator::new(clock_a, clock_b)?), + last_offsets: UnorderedRingBuffer::default(), + }); + + Ok((self, id)) + } + + pub fn add_untracked_link( + mut self, + clock_a: ClockId, + clock_b: ClockId, + ) -> Result<(Self, LinkId), LinkFilterError> { + if clock_a == clock_b { + return Err(LinkFilterError::ClocksEqual); + } + + let clock_a_internal = self.estimation_state.is_internal_clock(clock_a); + let clock_a_external = self.estimation_state.is_external_clock(clock_a); + let clock_b_internal = self.estimation_state.is_internal_clock(clock_b); + let clock_b_external = self.estimation_state.is_external_clock(clock_b); + + if !(clock_a_internal || clock_a_external) || !(clock_b_internal || clock_b_external) { + return Err(LinkFilterError::UnknownClock); + } + + if clock_a_external && clock_b_external { + return Err(LinkFilterError::BothClocksExternal); + } + + let id = LinkId::new(); + let is_internal = clock_a_internal && clock_b_internal; + self.links.push(LinkInfo { + id, + active: is_internal, + internal: is_internal, + link_state: LinkState::Untracked(clock_a, clock_b), + last_offsets: UnorderedRingBuffer::default(), + }); + + Ok((self, id)) + } + + pub fn remove_link(mut self, id: LinkId) -> Result { + let Some(info_index) = self.links.iter().position(|info| info.id == id) else { + return Err(LinkFilterError::UnknownLink); + }; + + let link = self.links.remove(info_index); + if link.active && matches!(link.link_state, LinkState::Tracked(_)) { + self.estimation_state = self.estimation_state.remove_link(id)?; + } + + Ok(self) + } +} diff --git a/statime-algo/src/lib.rs b/statime-algo/src/lib.rs new file mode 100644 index 000000000..52114dcc5 --- /dev/null +++ b/statime-algo/src/lib.rs @@ -0,0 +1,56 @@ +//! General datastructures as defined by the ptp spec +#![no_std] + +#[cfg(feature = "std")] +extern crate std; + +/// Unique identifier for a clock +// FIXME: Move to statime-base once that exists +#[derive(Copy, Clone, PartialEq, Eq, PartialOrd, Ord, Debug, Hash)] +pub struct ClockId(usize); + +impl ClockId { + /// Get a new identifier for a clock. + pub fn new() -> ClockId { + static COUNTER: AtomicUsize = AtomicUsize::new(0); + ClockId(COUNTER.fetch_add(1, core::sync::atomic::Ordering::Relaxed)) + } +} + +/// Unique identifier for a clock +// FIXME: Move to statime-base once that exists +#[derive(Copy, Clone, PartialEq, Eq, PartialOrd, Ord, Debug, Hash)] +pub struct LinkId(usize); + +impl LinkId { + /// Get a new identifier for a clock. + pub fn new() -> LinkId { + static COUNTER: AtomicUsize = AtomicUsize::new(0); + LinkId(COUNTER.fetch_add(1, core::sync::atomic::Ordering::Relaxed)) + } +} + +#[cfg(test)] +macro_rules! assert_almost_eq { + ($left:expr, $right:expr) => { + match (&$left, &$right) { + (left_val, right_val) => { + assert!( + (*left_val - *right_val).abs() <= 1e-6 * right_val.abs(), + "Floating point values not almost equal.\nLeft={left_val}\nRight={right_val}" + ) + } + } + }; +} + +mod estimator; +mod filter; +mod link_noise; +mod matrix; +mod ringbuffer; + +use core::sync::atomic::AtomicUsize; + +pub use estimator::{EstimatorError, EstimatorState}; +pub use link_noise::{LinkNoiseError, LinkNoiseEstimator}; diff --git a/statime-algo/src/link_noise.rs b/statime-algo/src/link_noise.rs new file mode 100644 index 000000000..2fccc619f --- /dev/null +++ b/statime-algo/src/link_noise.rs @@ -0,0 +1,213 @@ +use crate::{ + ClockId, LinkNoiseError::NotEnoughMeasurements, link_noise::LinkNoiseError::InvalidClocks, ringbuffer::UnorderedRingBuffer, +}; + +const DELAYS: usize = 8; +const MIN_DELAYS_FOR_ESTIMATES: usize = 4; +/// FIXME: Consider whether we want this configurable. +const MAX_TIME_BETWEEN_HALVES: f64 = 0.5; + +type Timestamp = f64; + +#[derive(Debug, Copy, Clone, PartialEq)] +struct PreviousMeasurement { + time: Timestamp, + offset: f64, + from: ClockId, + to: ClockId, +} + +/// An error that occured during link noise estimation +#[derive(Debug, Copy, Clone, PartialEq, Eq, Hash)] +pub enum LinkNoiseError { + /// One of the provided clocks is not a clock on this link. + InvalidClocks, + /// Both clocks in the link or in the measurement are the same. + ClocksEqual, + /// There are insufficient measurements to provide estimates. + NotEnoughMeasurements, +} + +/// Estimator for the noise induced by a given link +#[derive(Debug, Clone)] +pub struct LinkNoiseEstimator { + a: ClockId, + b: ClockId, + roundtrip_delays: UnorderedRingBuffer, + prev_measurement: Option, +} + +impl LinkNoiseEstimator { + /// Create a new estimator for the noise on a link between clocks A and B. + pub fn new(a: ClockId, b: ClockId) -> Result { + if a == b { + Err(LinkNoiseError::ClocksEqual) + } else { + Ok(LinkNoiseEstimator { + a, + b, + roundtrip_delays: UnorderedRingBuffer::default(), + prev_measurement: None, + }) + } + } + + /// Use a measurement on the link to update our estimates for the noise on the link. + pub fn measurement( + mut self, + from: ClockId, + to: ClockId, + offset: f64, + time: Timestamp, + ) -> Result { + if (from != self.a && from != self.b) || (to != self.a && to != self.b) { + return Err(LinkNoiseError::InvalidClocks); + } + + if from == to { + return Err(LinkNoiseError::ClocksEqual); + } + + if let Some(prev_measurement) = self.prev_measurement.take() + && prev_measurement.from == to + && prev_measurement.to == from + && time - prev_measurement.time < MAX_TIME_BETWEEN_HALVES + { + self.roundtrip_delays + .insert(prev_measurement.offset + offset); + } else { + self.prev_measurement = Some(PreviousMeasurement { + time, + offset, + from, + to, + }) + } + + Ok(self) + } + + /// The current estimate of the noise on the link + /// + /// Errors: + /// The noise estimate is only available if sufficient measurements have + /// occured for a reliable estimate to be made. + pub fn noise_estimate(&self) -> Result { + let roundtrip_delays = self.roundtrip_delays.as_ref(); + if roundtrip_delays.len() < MIN_DELAYS_FOR_ESTIMATES { + return Err(LinkNoiseError::NotEnoughMeasurements); + } + let mean = roundtrip_delays.iter().sum::() / (roundtrip_delays.len() as f64); + + let variance = roundtrip_delays + .iter() + .map(|f| (f - mean).powi(2)) + .sum::() + / ((roundtrip_delays.len() - 1) as f64); + + Ok((variance / 2.0).sqrt()) + } + + /// The current estimate of the delay on the link + /// + /// Errors: + /// The delay estimate is only available if sufficient measurements have + /// occured for a reliable estimate to be made. + pub fn delay_estimate(&self) -> Result { + let roundtrip_delays = self.roundtrip_delays.as_ref(); + if roundtrip_delays.len() < MIN_DELAYS_FOR_ESTIMATES { + return Err(LinkNoiseError::NotEnoughMeasurements); + } + + Ok(roundtrip_delays.iter().sum::() / ((2 * roundtrip_delays.len()) as f64)) + } +} + +#[cfg(test)] +mod tests { + use crate::{ClockId, estimator::UncertainValue, link_noise::LinkNoiseEstimator}; + + #[test] + fn link_noise_measures_link_noise_1() { + let state = LinkNoiseEstimator::new(ClockId(1), ClockId(2)) + .unwrap() + .measurement(ClockId(1), ClockId(2), 1.0, 0.0) + .unwrap() + .measurement(ClockId(2), ClockId(1), 1.0, 0.0) + .unwrap() + .measurement(ClockId(1), ClockId(2), 1.0, 0.0) + .unwrap() + .measurement(ClockId(2), ClockId(1), 1.0, 0.0) + .unwrap() + .measurement(ClockId(1), ClockId(2), 1.0, 0.0) + .unwrap() + .measurement(ClockId(2), ClockId(1), 1.0, 0.0) + .unwrap() + .measurement(ClockId(1), ClockId(2), 1.0, 0.0) + .unwrap() + .measurement(ClockId(2), ClockId(1), 1.0, 0.0) + .unwrap(); + + assert_eq!(state.noise_estimate().unwrap(), 0.0); + assert_eq!(state.delay_estimate().unwrap(), 1.0); + } + + #[test] + fn link_noise_measures_link_noise_2() { + let state = LinkNoiseEstimator::new(ClockId(1), ClockId(2)) + .unwrap() + .measurement(ClockId(1), ClockId(2), 1.0, 0.0) + .unwrap() + .measurement(ClockId(2), ClockId(1), 1.0, 0.0) + .unwrap() + .measurement(ClockId(1), ClockId(2), 1.0, 0.0) + .unwrap() + .measurement(ClockId(2), ClockId(1), 1.0, 0.0) + .unwrap() + .measurement(ClockId(1), ClockId(2), 0.5, 0.0) + .unwrap() + .measurement(ClockId(2), ClockId(1), 0.5, 0.0) + .unwrap() + .measurement(ClockId(1), ClockId(2), 0.5, 0.0) + .unwrap() + .measurement(ClockId(2), ClockId(1), 0.5, 0.0) + .unwrap(); + + assert_almost_eq!(state.noise_estimate().unwrap(), 1.0 / (6.0f64.sqrt())); + assert_eq!(state.delay_estimate().unwrap(), 0.75); + } + + /// Returns a link noise estimator with 0 link noise. + #[test] + fn link_noise_measures_link_noise_3() { + let a = ClockId(1); + let b = ClockId(2); + let delay: UncertainValue = (1.5, 0.1).into(); + + let state = LinkNoiseEstimator::new(a, b) + .unwrap() + .measurement(a, b, delay.value, 0.0) + .unwrap() + .measurement(b, a, delay.value, 0.0) + .unwrap() + .measurement(a, b, delay.value + delay.uncertainty / 2.0f64.sqrt(), 0.0) + .unwrap() + .measurement(b, a, delay.value + delay.uncertainty / 2.0f64.sqrt(), 0.0) + .unwrap() + .measurement(a, b, delay.value + delay.uncertainty / 2.0f64.sqrt(), 0.0) + .unwrap() + .measurement(b, a, delay.value + delay.uncertainty / 2.0f64.sqrt(), 0.0) + .unwrap() + .measurement(a, b, delay.value - delay.uncertainty / 2.0f64.sqrt(), 0.0) + .unwrap() + .measurement(b, a, delay.value - delay.uncertainty / 2.0f64.sqrt(), 0.0) + .unwrap() + .measurement(a, b, delay.value - delay.uncertainty / 2.0f64.sqrt(), 0.0) + .unwrap() + .measurement(b, a, delay.value - delay.uncertainty / 2.0f64.sqrt(), 0.0) + .unwrap(); + + assert_almost_eq!(state.delay_estimate().unwrap(), delay.value); + assert_almost_eq!(state.noise_estimate().unwrap(), delay.uncertainty); + } +} diff --git a/statime-algo/src/matrix.rs b/statime-algo/src/matrix.rs new file mode 100644 index 000000000..03d89c993 --- /dev/null +++ b/statime-algo/src/matrix.rs @@ -0,0 +1,556 @@ +use core::ops::{Add, AddAssign, Div, Index, IndexMut, Mul, Sub, SubAssign}; + +/// A storage provider for a matrix. Abstracts a dynamically sized array of f64. +/// +/// It is explicitly allowed for the [`AsRef`] and [`AsMut`] implementations to +/// return references to larger arrays, so long as the additional length is always +/// identical and modification to the additional entries does not matter. +pub trait MatrixStorage: AsRef<[f64]> + AsMut<[f64]> { + /// Create a new instance of the storage. + fn new(len: usize, data: impl FnMut(usize) -> f64) -> Self; +} + +#[cfg(feature = "std")] +impl MatrixStorage for std::boxed::Box<[f64]> { + fn new(len: usize, data: impl FnMut(usize) -> f64) -> Self { + (0..len).map(data).collect() + } +} + +impl MatrixStorage for [f64; N] { + fn new(len: usize, mut data: impl FnMut(usize) -> f64) -> Self { + assert!(len <= N); + core::array::from_fn(|index| if index < len { data(index) } else { 0.0 }) + } +} + +/// An error occured while performing a matrix operation. +#[derive(Debug, Clone, PartialEq, Eq, Hash)] +pub enum MatrixError { + /// The matrix is not a vector, but the operation requires it to be. + NotAVector, + /// The matrix is not square, but the operation requires it to be. + NotSquare, + /// The operation would have resulted in an out-of-bounds access. + OutOfBounds, +} + +#[derive(Debug, Copy, Clone, PartialEq)] +/// A simple class for computing with matrices. +/// +/// Indexing into these matrices is done with tuples indicating row first, and then the column. +pub struct Matrix { + rows: usize, + cols: usize, + storage: Storage, +} + +impl Matrix { + /// Number of rows in the matrix + pub fn rows(&self) -> usize { + self.rows + } + + /// Number of columns in the matrix + pub fn cols(&self) -> usize { + self.cols + } +} + +impl Matrix { + /// Create a new matrix, filling the values of the cells using the provided function. + pub fn new(rows: usize, cols: usize, mut values: impl FnMut(usize, usize) -> f64) -> Self { + Matrix { + rows, + cols, + storage: Storage::new(rows * cols, |index| values(index / cols, index % cols)), + } + } + + /// Create a new matrix with a single column (i.e. a vector), filling the + /// values of the cells using the provided function. + pub fn new_vec(rows: usize, values: impl FnMut(usize) -> f64) -> Self { + Matrix { + rows, + cols: 1, + storage: Storage::new(rows, values), + } + } + + /// Given that the current matrix is a vector, remove a portion of the vector. + pub fn splice_vec(&self, start: usize, length: usize) -> Result { + if self.cols != 1 { + return Err(MatrixError::NotAVector); + } + + if start + length > self.rows { + return Err(MatrixError::OutOfBounds); + } + + Ok(Matrix::new_vec(self.rows() - length, |row| { + if row < start { + self[(row, 0)] + } else { + self[(row + length, 0)] + } + })) + } + + /// Given that the current matrix is square, remove a portion of the matrix. + /// + /// The portion of the matrix is defined by a starting row/column and a length. + /// The removed portion starts at the starting column/row and extends for length rows and columns. + pub fn splice_square(&self, start: usize, length: usize) -> Result { + if self.rows != self.cols { + return Err(MatrixError::NotSquare); + } + + if start + length > self.rows { + return Err(MatrixError::OutOfBounds); + } + + Ok(Matrix::new( + self.rows - length, + self.cols - length, + |row, col| { + let row = if row < start { row } else { row + length }; + let col = if col < start { col } else { col + length }; + self[(row, col)] + }, + )) + } + + pub fn extend_vec(&self, values: [f64; ROWS]) -> Result { + if self.cols != 1 { + return Err(MatrixError::NotAVector); + } + + let original_rows = self.rows(); + Ok(Matrix::new_vec(original_rows + ROWS, |row| { + if row < original_rows { + self[(row, 0)] + } else { + values[row - original_rows] + } + })) + } + + pub fn extend(&self, data: [[f64; COLS]; ROWS]) -> Self { + let original_rows = self.rows(); + let original_cols = self.cols(); + Matrix::new(original_rows + ROWS, original_cols + COLS, |row, col| { + if row < original_rows && col < original_cols { + self[(row, col)] + } else if row >= original_rows && col >= original_cols { + data[row - original_rows][col - original_cols] + } else { + 0.0 + } + }) + } + + pub fn identity(size: usize) -> Self { + Matrix::new( + size, + size, + |row, column| if row == column { 1.0 } else { 0.0 }, + ) + } + + pub fn zero(rows: usize, cols: usize) -> Self { + Matrix::new(rows, cols, |_, _| 0.0) + } + + pub fn transpose(&self) -> Self { + Matrix::new(self.cols, self.rows, |row, column| self[(column, row)]) + } + + pub fn symmetrize(&self) -> Result { + if self.rows != self.cols { + return Err(MatrixError::NotSquare); + } + + // We can get away here without branching because floating point addition is + // symmetric. (IEEE 754, which is used in rust per the reference). + Ok(Matrix::new(self.rows, self.cols, |r, c| { + f64::midpoint(self[(r, c)], self[(c, r)]) + })) + } +} + +impl From for Matrix { + fn from(value: f64) -> Self { + Matrix::new(1, 1, |_, _| value) + } +} + +impl Index<(usize, usize)> for Matrix { + type Output = f64; + + fn index(&self, (r, c): (usize, usize)) -> &Self::Output { + assert!(r < self.rows); + assert!(c < self.cols); + &self.storage.as_ref()[r * self.cols + c] + } +} + +impl IndexMut<(usize, usize)> for Matrix { + fn index_mut(&mut self, (r, c): (usize, usize)) -> &mut Self::Output { + assert!(r < self.rows); + assert!(c < self.cols); + &mut self.storage.as_mut()[r * self.cols + c] + } +} + +impl Add<&Matrix> for &Matrix { + type Output = Matrix; + + fn add(self, rhs: &Matrix) -> Self::Output { + assert_eq!(self.cols, rhs.cols); + assert_eq!(self.rows, rhs.rows); + + let lhs = self.storage.as_ref(); + let rhs = rhs.storage.as_ref(); + + Matrix { + rows: self.rows, + cols: self.cols, + storage: Storage::new(self.rows * self.cols, |index| lhs[index] + rhs[index]), + } + } +} + +impl Add> for &Matrix { + type Output = Matrix; + + fn add(self, rhs: Matrix) -> Self::Output { + self + &rhs + } +} + +impl Add<&Matrix> for Matrix { + type Output = Matrix; + + fn add(self, rhs: &Matrix) -> Self::Output { + &self + rhs + } +} + +impl Add> for Matrix { + type Output = Matrix; + + fn add(self, rhs: Matrix) -> Self::Output { + &self + &rhs + } +} + +impl AddAssign<&Matrix> for Matrix { + fn add_assign(&mut self, rhs: &Matrix) { + assert_eq!(self.cols, rhs.cols); + assert_eq!(self.rows, rhs.rows); + let lhs = self.storage.as_mut(); + let rhs = rhs.storage.as_ref(); + for (i, value) in lhs.iter_mut().enumerate() { + *value += rhs[i]; + } + } +} + +impl AddAssign> for Matrix { + fn add_assign(&mut self, rhs: Matrix) { + *self += &rhs; + } +} + +impl Sub<&Matrix> for &Matrix { + type Output = Matrix; + + fn sub(self, rhs: &Matrix) -> Self::Output { + assert_eq!(self.cols, rhs.cols); + assert_eq!(self.rows, rhs.rows); + + let lhs = self.storage.as_ref(); + let rhs = rhs.storage.as_ref(); + + Matrix { + rows: self.rows, + cols: self.cols, + storage: Storage::new(self.rows * self.cols, |index| lhs[index] - rhs[index]), + } + } +} + +impl Sub<&Matrix> for Matrix { + type Output = Matrix; + + fn sub(self, rhs: &Matrix) -> Self::Output { + &self - rhs + } +} + +impl Sub> for &Matrix { + type Output = Matrix; + + fn sub(self, rhs: Matrix) -> Self::Output { + self - &rhs + } +} + +impl Sub> for Matrix { + type Output = Matrix; + + fn sub(self, rhs: Matrix) -> Self::Output { + &self - &rhs + } +} + +impl SubAssign<&Matrix> for Matrix { + fn sub_assign(&mut self, rhs: &Matrix) { + assert_eq!(self.cols, rhs.cols); + assert_eq!(self.rows, rhs.rows); + let lhs = self.storage.as_mut(); + let rhs = rhs.storage.as_ref(); + for (i, value) in lhs.iter_mut().enumerate() { + *value -= rhs[i]; + } + } +} + +impl SubAssign> for Matrix { + fn sub_assign(&mut self, rhs: Matrix) { + *self -= &rhs; + } +} + +impl Mul<&Matrix> for f64 { + type Output = Matrix; + + fn mul(self, rhs: &Matrix) -> Self::Output { + let rows = rhs.rows; + let cols = rhs.cols; + let rhs = rhs.storage.as_ref(); + + Matrix { + rows, + cols, + storage: Storage::new(rows * cols, |index| rhs[index] * self), + } + } +} + +impl Mul> for f64 { + type Output = Matrix; + + fn mul(self, rhs: Matrix) -> Self::Output { + self * &rhs + } +} + +impl Mul for &Matrix { + type Output = Matrix; + + fn mul(self, rhs: f64) -> Self::Output { + let lhs = self.storage.as_ref(); + + Matrix { + rows: self.rows, + cols: self.cols, + storage: Storage::new(self.rows * self.cols, |index| lhs[index] * rhs), + } + } +} + +impl Mul for Matrix { + type Output = Matrix; + + fn mul(self, rhs: f64) -> Self::Output { + &self * rhs + } +} + +impl Div for &Matrix { + type Output = Matrix; + + fn div(self, rhs: f64) -> Self::Output { + let lhs = self.storage.as_ref(); + + Matrix { + rows: self.rows, + cols: self.cols, + #[expect( + clippy::suspicious_arithmetic_impl, + reason = "False positive, the multiplication is required for division" + )] + storage: Storage::new(self.rows * self.cols, |index| lhs[index] / rhs), + } + } +} + +impl Div for Matrix { + type Output = Matrix; + + fn div(self, rhs: f64) -> Self::Output { + &self / rhs + } +} + +impl Mul<&Matrix> for &Matrix { + type Output = Matrix; + + fn mul(self, rhs: &Matrix) -> Self::Output { + assert_eq!(self.cols, rhs.rows); + + let lhs_storage = self.storage.as_ref(); + let rhs_storage = rhs.storage.as_ref(); + + Matrix { + rows: self.rows, + cols: rhs.cols, + storage: Storage::new(self.rows * rhs.cols, |index| { + let r = index / rhs.cols; + let c = index % rhs.cols; + (0..self.cols) + .map(|k| lhs_storage[r * self.cols + k] * rhs_storage[k * rhs.cols + c]) + .sum::() + }), + } + } +} + +impl Mul<&Matrix> for Matrix { + type Output = Matrix; + + fn mul(self, rhs: &Matrix) -> Self::Output { + &self * rhs + } +} + +impl Mul> for &Matrix { + type Output = Matrix; + + fn mul(self, rhs: Matrix) -> Self::Output { + self * &rhs + } +} + +impl Mul> for Matrix { + type Output = Matrix; + + fn mul(self, rhs: Matrix) -> Self::Output { + &self * &rhs + } +} + +#[cfg(all(test, feature = "std"))] +#[allow(clippy::cast_precision_loss, reason = "Test code")] +#[allow(clippy::float_cmp, reason = "Test code")] +mod tests { + use super::Matrix; + use std::boxed::Box; + + #[test] + fn test_indexing() { + let mut matrix = Matrix::>::new(3, 2, |r, c| (r * 100 + c) as f64); + + assert_eq!(matrix[(0, 0)], 0.0); + assert_eq!(matrix[(1, 0)], 100.0); + assert_eq!(matrix[(2, 0)], 200.0); + assert_eq!(matrix[(0, 1)], 1.0); + assert_eq!(matrix[(1, 1)], 101.0); + assert_eq!(matrix[(2, 1)], 201.0); + + matrix[(1, 0)] = 50.0; + + assert_eq!(matrix[(0, 0)], 0.0); + assert_eq!(matrix[(1, 0)], 50.0); + assert_eq!(matrix[(2, 0)], 200.0); + assert_eq!(matrix[(0, 1)], 1.0); + assert_eq!(matrix[(1, 1)], 101.0); + assert_eq!(matrix[(2, 1)], 201.0); + } + + #[test] + fn test_array_storage() { + let matrix = Matrix::<[f64; 10]>::new(3, 2, |r, c| (r * 100 + c) as f64); + + assert_eq!(matrix[(0, 0)], 0.0); + assert_eq!(matrix[(1, 0)], 100.0); + assert_eq!(matrix[(2, 0)], 200.0); + assert_eq!(matrix[(0, 1)], 1.0); + assert_eq!(matrix[(1, 1)], 101.0); + assert_eq!(matrix[(2, 1)], 201.0); + } + + #[test] + fn test_add() { + let mut matrix = Matrix::>::new(2, 3, |r, c| (r * 50 + c * 2) as f64) + + Matrix::>::new(2, 3, |r, c| (r * 150 + c * 3) as f64); + + assert_eq!(matrix[(0, 0)], 0.0); + assert_eq!(matrix[(1, 0)], 200.0); + assert_eq!(matrix[(0, 1)], 5.0); + assert_eq!(matrix[(1, 1)], 205.0); + assert_eq!(matrix[(0, 2)], 10.0); + assert_eq!(matrix[(1, 2)], 210.0); + + matrix += Matrix::>::new(2, 3, |r, c| (r * 75 + c * 4) as f64); + + assert_eq!(matrix[(0, 0)], 0.0); + assert_eq!(matrix[(1, 0)], 275.0); + assert_eq!(matrix[(0, 1)], 9.0); + assert_eq!(matrix[(1, 1)], 284.0); + assert_eq!(matrix[(0, 2)], 18.0); + assert_eq!(matrix[(1, 2)], 293.0); + } + + #[test] + fn test_sub() { + let mut matrix = Matrix::>::new(1, 2, |_, c| (100 + 50 * c) as f64) + - Matrix::>::new(1, 2, |_, c| (15 + 75 * c) as f64); + + assert_eq!(matrix[(0, 0)], 85.0); + assert_eq!(matrix[(0, 1)], 60.0); + + matrix -= Matrix::>::new(1, 2, |_, _| 9.0); + + assert_eq!(matrix[(0, 0)], 76.0); + assert_eq!(matrix[(0, 1)], 51.0); + } + + #[test] + fn test_mul() { + let matrix = 2.5 * Matrix::>::new(2, 3, |r, c| (r * 50 + c * 2) as f64); + + assert_eq!(matrix[(0, 0)], 0.0); + assert_eq!(matrix[(1, 0)], 125.0); + assert_eq!(matrix[(0, 1)], 5.0); + assert_eq!(matrix[(1, 1)], 130.0); + assert_eq!(matrix[(0, 2)], 10.0); + assert_eq!(matrix[(1, 2)], 135.0); + + let matrix = Matrix::>::new(2, 3, |r, c| (r * 50 + c * 2) as f64) * 2.5; + + assert_eq!(matrix[(0, 0)], 0.0); + assert_eq!(matrix[(1, 0)], 125.0); + assert_eq!(matrix[(0, 1)], 5.0); + assert_eq!(matrix[(1, 1)], 130.0); + assert_eq!(matrix[(0, 2)], 10.0); + assert_eq!(matrix[(1, 2)], 135.0); + + let matrix = Matrix::>::new(2, 2, |r, c| if r == c { 0.0 } else { 1.0 }) + * Matrix::>::new(2, 2, |r, c| (r * 2 + c) as f64); + + assert_eq!(matrix[(0, 0)], 2.0); + assert_eq!(matrix[(0, 1)], 3.0); + assert_eq!(matrix[(1, 0)], 0.0); + assert_eq!(matrix[(1, 1)], 1.0); + + let matrix = Matrix::>::new(2, 2, |r, c| (r * 2 + c) as f64) + * Matrix::>::new(2, 2, |r, c| if r == c { 0.0 } else { 1.0 }); + + assert_eq!(matrix[(0, 0)], 1.0); + assert_eq!(matrix[(0, 1)], 0.0); + assert_eq!(matrix[(1, 0)], 3.0); + assert_eq!(matrix[(1, 1)], 2.0); + } +} diff --git a/statime-algo/src/ringbuffer.rs b/statime-algo/src/ringbuffer.rs new file mode 100644 index 000000000..15dab6f2d --- /dev/null +++ b/statime-algo/src/ringbuffer.rs @@ -0,0 +1,22 @@ +const RINGBUFFER_SIZE: usize = 8; + +#[derive(Debug, Clone, PartialEq, Default)] +pub(crate) struct UnorderedRingBuffer { + values: [f64; RINGBUFFER_SIZE], + n_values: usize, + write_idx: usize, +} + +impl UnorderedRingBuffer { + pub(crate) fn insert(&mut self, value: f64) { + self.values[self.write_idx] = value; + self.write_idx = (self.write_idx + 1) % RINGBUFFER_SIZE; + self.n_values = (self.n_values + 1).min(RINGBUFFER_SIZE); + } +} + +impl AsRef<[f64]> for UnorderedRingBuffer { + fn as_ref(&self) -> &[f64] { + &self.values[..self.n_values] + } +}