From 49014d4c9960f9777ac8cddb953412cb80e17f8e Mon Sep 17 00:00:00 2001 From: David Venhoek Date: Mon, 6 Jul 2026 09:34:39 +0200 Subject: [PATCH 01/18] Initial implementation of matrix math for new algorithm. --- Cargo.lock | 4 + Cargo.toml | 2 +- statime-algo/Cargo.toml | 20 +++ statime-algo/src/lib.rs | 7 + statime-algo/src/matrix.rs | 291 +++++++++++++++++++++++++++++++++++++ 5 files changed, 323 insertions(+), 1 deletion(-) create mode 100644 statime-algo/Cargo.toml create mode 100644 statime-algo/src/lib.rs create mode 100644 statime-algo/src/matrix.rs 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/lib.rs b/statime-algo/src/lib.rs new file mode 100644 index 000000000..7de753589 --- /dev/null +++ b/statime-algo/src/lib.rs @@ -0,0 +1,7 @@ +//! General datastructures as defined by the ptp spec +#![no_std] + +#[cfg(feature = "std")] +extern crate std; + +mod matrix; diff --git a/statime-algo/src/matrix.rs b/statime-algo/src/matrix.rs new file mode 100644 index 000000000..729d5448a --- /dev/null +++ b/statime-algo/src/matrix.rs @@ -0,0 +1,291 @@ +use core::ops::{Add, AddAssign, 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 }) + } +} + +#[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 { + /// 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)), + } + } +} + +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> 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 AddAssign> 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 Sub> 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 SubAssign> 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 Mul> 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 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: 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::() + }), + } + } +} + +#[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); + } +} From 1d632cabb04668bff11971ba76c28841a00f981d Mon Sep 17 00:00:00 2001 From: David Venhoek Date: Sat, 18 Jul 2026 08:48:40 +0200 Subject: [PATCH 02/18] WIP --- statime-algo/src/estimator.rs | 387 ++++++++++++++++++++++++++++++++++ statime-algo/src/lib.rs | 6 + statime-algo/src/matrix.rs | 28 +++ 3 files changed, 421 insertions(+) create mode 100644 statime-algo/src/estimator.rs diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs new file mode 100644 index 000000000..5dc212bdd --- /dev/null +++ b/statime-algo/src/estimator.rs @@ -0,0 +1,387 @@ +use std::{boxed::Box, vec::Vec}; + +use crate::matrix::Matrix; + +use super::{ClockId, LinkId}; + +//FIXME: Replace with proper Timestamp type +type Timestamp = f64; + +//FIXME: Make more permanent error enum +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub(crate) enum EstimatorError { + ClockNotFound, + ClockAlreadyExists, + LinkNotFound, + LinkAlreadyExists, +} + +#[derive(Debug, Clone, Copy)] +struct ClockInfo { + id: ClockId, + base_index: usize, + wander: f64, +} + +impl ClockInfo { + fn offset_index(self) -> usize { + self.base_index + } + + fn frequency_index(self) -> usize { + self.base_index + 1 + } +} + +#[derive(Debug, Clone, Copy)] +struct LinkInfo { + id: LinkId, + index: usize, +} + +#[derive(Debug, Clone)] +struct EstimatorState { + time: Timestamp, + state: Matrix>, + uncertainty: Matrix>, + clock_info: Vec, + link_info: Vec, +} + +#[derive(Debug, Clone, Copy, PartialEq)] +struct UncertainValue { + /// Best estimate of the value + value: f64, + /// Square root of the variance of the value. Corresponds + /// to 1 standard deviation. + uncertainty: f64, +} + +impl From<(f64, f64)> for UncertainValue { + fn from(value: (f64, f64)) -> Self { + UncertainValue { + value: value.0, + uncertainty: value.1, + } + } +} + +impl EstimatorState { + pub fn empty(time: Timestamp) -> EstimatorState { + EstimatorState { + time, + state: Matrix::zero(0, 1), + uncertainty: Matrix::zero(0, 0), + clock_info: Vec::new(), + link_info: Vec::new(), + } + } + + pub fn progress_time(&self, new_time: Timestamp) -> EstimatorState { + 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 clock_info in &self.clock_info { + 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); + } + + EstimatorState { + time: new_time, + state: update.clone() * self.state.clone(), + uncertainty: update.clone() * self.uncertainty.clone() * update.transpose() + noise, + clock_info: self.clock_info.clone(), + link_info: self.link_info.clone(), + } + } + + // Assumes it is happening NOW with respect to the time of the previous estimate + pub fn measurement( + &self, + from: ClockId, + to: ClockId, + offset: UncertainValue, + link_delay: Option, + ) -> Result { + todo!() + } + + pub fn add_clock( + &self, + id: ClockId, + initial_offset: UncertainValue, + initial_frequency: UncertainValue, + initial_wander: f64, + ) -> Result { + if self.clock_info.iter().any(|info| info.id == id) { + return Err(EstimatorError::ClockAlreadyExists); + } + + let new_clock_info = ClockInfo { + id, + base_index: self.state.rows(), + wander: initial_wander, + }; + + let mut clock_info = self.clock_info.clone(); + clock_info.push(new_clock_info); + + Ok(EstimatorState { + time: self.time, + state: Matrix::new(self.state.rows() + 2, 1, |row, _| { + if row == new_clock_info.offset_index() { + initial_offset.value + } else if row == new_clock_info.frequency_index() { + initial_frequency.value + } else { + self.state[(row, 0)] + } + }), + uncertainty: Matrix::new( + self.state.rows() + 2, + self.state.rows() + 2, + |row, column| { + if row < self.uncertainty.rows() && column < self.uncertainty.cols() { + // Existing uncertainty + self.uncertainty[(row, column)] + } else if row == column && row == new_clock_info.offset_index() { + // New clock has only uncertainty on the diagonal, for offset + initial_offset.uncertainty.powi(2) + } else if row == column && row == new_clock_info.frequency_index() { + // and frequency. No correlations between those yet. + initial_frequency.uncertainty.powi(2) + } else { + // No correlations between uncertainty of new clock and old state yet. + 0.0 + } + }, + ), + clock_info, + link_info: self.link_info.clone(), + }) + } + + pub fn remove_clock(&self, id: ClockId) -> Result { + let clock_info = self.get_clock_info(id)?; + + Ok(EstimatorState { + time: self.time, + state: Matrix::new(self.state.rows() - 2, 1, |row, _| { + if row < clock_info.base_index { + self.state[(row, 0)] + } else { + self.state[(row + 2, 0)] + } + }), + uncertainty: Matrix::new( + self.uncertainty.rows() - 2, + self.uncertainty.cols() - 2, + |row, col| { + let row = if row < clock_info.base_index { + row + } else { + row + 2 + }; + let col = if col < clock_info.base_index { + col + } else { + col + 2 + }; + self.uncertainty[(row, col)] + }, + ), + clock_info: self + .clock_info + .iter() + .filter_map(|info| { + if info.id == id { + None + } else { + Some(ClockInfo { + id: info.id, + base_index: if info.base_index < clock_info.base_index { + info.base_index + } else { + info.base_index - 2 + }, + wander: info.wander, + }) + } + }) + .collect(), + link_info: self + .link_info + .iter() + .map(|link_info| LinkInfo { + id: link_info.id, + index: if link_info.index < clock_info.base_index { + link_info.index + } else { + link_info.index - 2 + }, + }) + .collect(), + }) + } + + pub fn add_link( + &self, + id: LinkId, + initial_delay: f64, + initial_delay_uncertainty: f64, + ) -> Result { + todo!() + } + + pub fn remove_link(&self, id: LinkId) -> Result { + todo!() + } + + 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(), + }) + } + + 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(), + }) + } +} + +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 crate::{ClockId, estimator::EstimatorState}; + + 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); + let state = state + .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); + } + + #[test] + fn test_time_evolve() { + let state = EstimatorState::empty(0.0); + let state = state + .add_clock(ClockId(1), (0.0, 0.0).into(), (1e-6, 0.0).into(), 1e-8) + .unwrap(); + let state = state + .add_clock(ClockId(2), (0.0, 1e-5).into(), (-1e-6, 1e-7).into(), 0.0) + .unwrap(); + let state = state.progress_time(100.0); + 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.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()) + ); + } + + #[test] + fn test_progress_time_composes_well() { + let state = EstimatorState::empty(0.0); + let state = state + .add_clock(ClockId(1), (0.0, 0.0).into(), (1e-6, 0.0).into(), 1e-8) + .unwrap(); + + let state_at_once = state.progress_time(100.0); + + let state_intermediate = state.progress_time(75.0); + let state_via_intermediate = state_intermediate.progress_time(100.0); + + 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() + ); + } +} diff --git a/statime-algo/src/lib.rs b/statime-algo/src/lib.rs index 7de753589..52d64a26d 100644 --- a/statime-algo/src/lib.rs +++ b/statime-algo/src/lib.rs @@ -4,4 +4,10 @@ #[cfg(feature = "std")] extern crate std; +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +struct ClockId(u64); +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +struct LinkId(u64); + mod matrix; +mod estimator; diff --git a/statime-algo/src/matrix.rs b/statime-algo/src/matrix.rs index 729d5448a..a8ba8b1c7 100644 --- a/statime-algo/src/matrix.rs +++ b/statime-algo/src/matrix.rs @@ -34,6 +34,18 @@ pub struct Matrix { 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 { @@ -43,6 +55,22 @@ impl Matrix { storage: Storage::new(rows * cols, |index| values(index / cols, index % cols)), } } + + 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)]) + } } impl Index<(usize, usize)> for Matrix { From d8dde6cc2fc3e666c27223b7157038256b8984b7 Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Sat, 18 Jul 2026 14:11:45 +0200 Subject: [PATCH 03/18] Simplified methods, add link add/remove --- statime-algo/src/estimator.rs | 223 +++++++++++++++++++++------------- statime-algo/src/matrix.rs | 93 ++++++++++++++ 2 files changed, 233 insertions(+), 83 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 5dc212bdd..354b04203 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -1,6 +1,6 @@ use std::{boxed::Box, vec::Vec}; -use crate::matrix::Matrix; +use crate::matrix::{Matrix, MatrixError}; use super::{ClockId, LinkId}; @@ -8,12 +8,19 @@ use super::{ClockId, LinkId}; type Timestamp = f64; //FIXME: Make more permanent error enum -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +#[derive(Debug, Clone, PartialEq, Eq, Hash)] pub(crate) enum EstimatorError { ClockNotFound, ClockAlreadyExists, LinkNotFound, LinkAlreadyExists, + MatrixError(MatrixError), +} + +impl From for EstimatorError { + fn from(err: MatrixError) -> Self { + EstimatorError::MatrixError(err) + } } #[derive(Debug, Clone, Copy)] @@ -31,6 +38,22 @@ impl ClockInfo { fn frequency_index(self) -> usize { self.base_index + 1 } + + fn with_index(self, new_base_index: usize) -> ClockInfo { + ClockInfo { + id: self.id, + base_index: new_base_index, + wander: self.wander, + } + } + + fn with_moved_index(self, from_index: usize, delta: usize) -> ClockInfo { + self.with_index(if self.base_index < from_index { + self.base_index + } else { + self.base_index - delta + }) + } } #[derive(Debug, Clone, Copy)] @@ -39,6 +62,23 @@ struct LinkInfo { index: usize, } +impl LinkInfo { + fn with_index(self, new_index: usize) -> LinkInfo { + LinkInfo { + id: self.id, + index: new_index, + } + } + + fn with_moved_index(self, from_index: usize, delta: usize) -> LinkInfo { + self.with_index(if self.index < from_index { + self.index + } else { + self.index - delta + }) + } +} + #[derive(Debug, Clone)] struct EstimatorState { time: Timestamp, @@ -48,6 +88,7 @@ struct EstimatorState { link_info: Vec, } +/// Represents an uncertain value, with a best estimate and an uncertainty (standard deviation). #[derive(Debug, Clone, Copy, PartialEq)] struct UncertainValue { /// Best estimate of the value @@ -57,6 +98,7 @@ struct UncertainValue { 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 { @@ -67,6 +109,9 @@ impl From<(f64, f64)> for UncertainValue { } impl EstimatorState { + /// Create a new empty estimator state at the given timestamp. + /// + /// This state has no clocks or links contained in it. pub fn empty(time: Timestamp) -> EstimatorState { EstimatorState { time, @@ -77,12 +122,14 @@ impl EstimatorState { } } + /// Progress the estimator state to the new timestamp. pub fn progress_time(&self, new_time: Timestamp) -> EstimatorState { 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 { 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, @@ -111,7 +158,10 @@ impl EstimatorState { } } - // Assumes it is happening NOW with respect to the time of the previous estimate + /// 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( &self, from: ClockId, @@ -122,6 +172,10 @@ impl EstimatorState { todo!() } + /// 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( &self, id: ClockId, @@ -144,115 +198,92 @@ impl EstimatorState { Ok(EstimatorState { time: self.time, - state: Matrix::new(self.state.rows() + 2, 1, |row, _| { - if row == new_clock_info.offset_index() { - initial_offset.value - } else if row == new_clock_info.frequency_index() { - initial_frequency.value - } else { - self.state[(row, 0)] - } - }), - uncertainty: Matrix::new( - self.state.rows() + 2, - self.state.rows() + 2, - |row, column| { - if row < self.uncertainty.rows() && column < self.uncertainty.cols() { - // Existing uncertainty - self.uncertainty[(row, column)] - } else if row == column && row == new_clock_info.offset_index() { - // New clock has only uncertainty on the diagonal, for offset - initial_offset.uncertainty.powi(2) - } else if row == column && row == new_clock_info.frequency_index() { - // and frequency. No correlations between those yet. - initial_frequency.uncertainty.powi(2) - } else { - // No correlations between uncertainty of new clock and old state yet. - 0.0 - } - }, - ), + state: self + .state + .extend_vec([initial_offset.value, initial_frequency.value])?, + uncertainty: self.uncertainty.extend([ + [initial_offset.uncertainty.powi(2), 0.0], + [0.0, initial_frequency.uncertainty.powi(2)], + ]), clock_info, link_info: self.link_info.clone(), }) } + /// Remove a clock from the estimator state. pub fn remove_clock(&self, id: ClockId) -> Result { let clock_info = self.get_clock_info(id)?; Ok(EstimatorState { time: self.time, - state: Matrix::new(self.state.rows() - 2, 1, |row, _| { - if row < clock_info.base_index { - self.state[(row, 0)] - } else { - self.state[(row + 2, 0)] - } - }), - uncertainty: Matrix::new( - self.uncertainty.rows() - 2, - self.uncertainty.cols() - 2, - |row, col| { - let row = if row < clock_info.base_index { - row - } else { - row + 2 - }; - let col = if col < clock_info.base_index { - col - } else { - col + 2 - }; - self.uncertainty[(row, col)] - }, - ), + state: self.state.splice_vec(clock_info.base_index, 2)?, + uncertainty: self.uncertainty.splice_square(clock_info.base_index, 2)?, clock_info: self .clock_info .iter() - .filter_map(|info| { - if info.id == id { - None - } else { - Some(ClockInfo { - id: info.id, - base_index: if info.base_index < clock_info.base_index { - info.base_index - } else { - info.base_index - 2 - }, - wander: info.wander, - }) - } - }) + .filter(|info| info.id != id) + .map(|info| info.with_moved_index(clock_info.base_index, 2)) .collect(), link_info: self .link_info .iter() - .map(|link_info| LinkInfo { - id: link_info.id, - index: if link_info.index < clock_info.base_index { - link_info.index - } else { - link_info.index - 2 - }, - }) + .map(|link_info| link_info.with_moved_index(clock_info.base_index, 2)) .collect(), }) } + /// Add a new link to the estimator state. pub fn add_link( &self, id: LinkId, - initial_delay: f64, - initial_delay_uncertainty: f64, + initial_delay: UncertainValue, ) -> Result { - todo!() + if self.link_info.iter().any(|info| info.id == id) { + return Err(EstimatorError::LinkAlreadyExists); + } + + let new_link_info = LinkInfo { + id, + index: self.state.rows(), + }; + + let mut link_info = self.link_info.clone(); + link_info.push(new_link_info); + + Ok(EstimatorState { + time: self.time, + state: self.state.extend_vec([initial_delay.value])?, + uncertainty: self + .uncertainty + .extend([[initial_delay.uncertainty.powi(2)]]), + clock_info: self.clock_info.clone(), + link_info, + }) } + /// Remove a link from the estimator state. pub fn remove_link(&self, id: LinkId) -> Result { - todo!() + let link_info = self.get_link_info(id)?; + + Ok(EstimatorState { + time: self.time, + state: self.state.splice_vec(link_info.index, 1)?, + uncertainty: self.uncertainty.splice_square(link_info.index, 1)?, + clock_info: self + .clock_info + .iter() + .map(|clock_info| clock_info.with_moved_index(link_info.index, 1)) + .collect(), + link_info: self + .link_info + .iter() + .filter(|info| info.id != id) + .map(|info| info.with_moved_index(link_info.index, 1)) + .collect(), + }) } + /// 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 { @@ -262,6 +293,7 @@ impl EstimatorState { }) } + /// Get the current freqency 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 { @@ -271,6 +303,15 @@ impl EstimatorState { .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(), + }) + } } impl EstimatorState { @@ -291,7 +332,7 @@ impl EstimatorState { #[cfg(test)] mod tests { - use crate::{ClockId, estimator::EstimatorState}; + use crate::{ClockId, LinkId, estimator::EstimatorState}; macro_rules! assert_almost_eq { ($left:expr, $right:expr) => { @@ -335,6 +376,7 @@ mod tests { let state = state .add_clock(ClockId(1), (0.0, 0.0).into(), (1e-6, 0.0).into(), 1e-8) .unwrap(); + let state = state.add_link(LinkId(1), (0.5, 0.2).into()).unwrap(); let state = state .add_clock(ClockId(2), (0.0, 1e-5).into(), (-1e-6, 1e-7).into(), 0.0) .unwrap(); @@ -354,6 +396,11 @@ mod tests { 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); @@ -384,4 +431,14 @@ mod tests { state_via_intermediate.clock_frequency(ClockId(1)).unwrap() ); } + + #[test] + fn test_add_link() { + let state = EstimatorState::empty(0.0); + let state = state + .add_link(LinkId(1), (1.0, 2.0).into()) + .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); + } } diff --git a/statime-algo/src/matrix.rs b/statime-algo/src/matrix.rs index a8ba8b1c7..4bc70cbe9 100644 --- a/statime-algo/src/matrix.rs +++ b/statime-algo/src/matrix.rs @@ -24,6 +24,17 @@ impl MatrixStorage for [f64; N] { } } +/// 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. /// @@ -56,6 +67,88 @@ impl Matrix { } } + /// 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, From dee6fe17e7f6abbd963040b1ed1927ad68664168 Mon Sep 17 00:00:00 2001 From: David Venhoek Date: Sat, 18 Jul 2026 19:53:55 +0200 Subject: [PATCH 04/18] Implemented measurements and beginnnings of external clocks. --- statime-algo/src/estimator.rs | 231 +++++++++++++++++++++++++++++++++- statime-algo/src/matrix.rs | 30 ++++- 2 files changed, 258 insertions(+), 3 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 354b04203..54c3fac7e 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -1,4 +1,4 @@ -use std::{boxed::Box, vec::Vec}; +use std::{boxed::Box, sync::Arc, vec::Vec}; use crate::matrix::{Matrix, MatrixError}; @@ -85,6 +85,7 @@ struct EstimatorState { state: Matrix>, uncertainty: Matrix>, clock_info: Vec, + external_clocks: Vec, link_info: Vec, } @@ -118,6 +119,7 @@ impl EstimatorState { state: Matrix::zero(0, 1), uncertainty: Matrix::zero(0, 0), clock_info: Vec::new(), + external_clocks: Vec::new(), link_info: Vec::new(), } } @@ -154,6 +156,7 @@ impl EstimatorState { state: update.clone() * self.state.clone(), uncertainty: update.clone() * self.uncertainty.clone() * update.transpose() + noise, clock_info: self.clock_info.clone(), + external_clocks: self.external_clocks.clone(), link_info: self.link_info.clone(), } } @@ -169,6 +172,67 @@ impl EstimatorState { offset: UncertainValue, link_delay: Option, ) -> Result { + let from_clock_info = self.get_clock_info(from)?; + let to_clock_info = self.get_clock_info(to)?; + + let mut measurement_projection = Matrix::zero(1, self.state.rows()); + measurement_projection[(0, from_clock_info.offset_index())] = -1.0; + 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.clone() * self.state.clone(); + 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.clone() + * self.uncertainty.clone() + * measurement_projection.transpose() + + offset.uncertainty.powi(2).into(); + + // 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.clone() * measurement_projection.transpose() + / difference_covariance[(0, 0)]; + + // This is simply using the strenght we calculated before to update the state + let new_state = self.state.clone() + update_strength.clone() * 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_proporitionality = + Matrix::identity(self.state.rows()) - update_strength.clone() * measurement_projection; + let new_uncertainty = (prev_step_proporitionality.clone() + * (self.uncertainty.clone() * prev_step_proporitionality.transpose()) + + update_strength.clone() * offset.uncertainty.powi(2) * update_strength.transpose()) + .symmetrize(); + + Ok(EstimatorState { + time: self.time, + state: new_state, + uncertainty: new_uncertainty, + clock_info: self.clock_info.clone(), + external_clocks: self.external_clocks.clone(), + link_info: self.link_info.clone(), + }) + } + + /// Add an external clock to the estimator state. + pub fn add_external_clock(&self, id: ClockId) -> Result { + todo!() + } + + pub fn remove_external_clock(&self, id: ClockId) -> Result { todo!() } @@ -206,6 +270,7 @@ impl EstimatorState { [0.0, initial_frequency.uncertainty.powi(2)], ]), clock_info, + external_clocks: self.external_clocks.clone(), link_info: self.link_info.clone(), }) } @@ -224,6 +289,7 @@ impl EstimatorState { .filter(|info| info.id != id) .map(|info| info.with_moved_index(clock_info.base_index, 2)) .collect(), + external_clocks: self.external_clocks.clone(), link_info: self .link_info .iter() @@ -257,6 +323,7 @@ impl EstimatorState { .uncertainty .extend([[initial_delay.uncertainty.powi(2)]]), clock_info: self.clock_info.clone(), + external_clocks: self.external_clocks.clone(), link_info, }) } @@ -274,6 +341,7 @@ impl EstimatorState { .iter() .map(|clock_info| clock_info.with_moved_index(link_info.index, 1)) .collect(), + external_clocks: self.external_clocks.clone(), link_info: self .link_info .iter() @@ -332,7 +400,10 @@ impl EstimatorState { #[cfg(test)] mod tests { - use crate::{ClockId, LinkId, estimator::EstimatorState}; + use crate::{ + ClockId, LinkId, + estimator::{EstimatorState, UncertainValue}, + }; macro_rules! assert_almost_eq { ($left:expr, $right:expr) => { @@ -441,4 +512,160 @@ mod tests { 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); + let state = state + .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap(); + let state = state + .add_clock(ClockId(2), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap(); + + let state = state + .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); + let state = state + .add_clock(ClockId(1), (0.0, 0.0).into(), (0.0, 1e-3).into(), 0.0) + .unwrap(); + let state = state + .add_clock(ClockId(2), (0.0, 0.0).into(), (0.0, 1e-3).into(), 0.0) + .unwrap(); + + let state = state.progress_time(100.0); + + let state = state + .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); + let state = state + .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap(); + let state = state + .add_clock(ClockId(2), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap(); + let state = state.add_link(LinkId(1), (1.0, 0.0).into()).unwrap(); + + let state = state + .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); + let state = state + .add_clock(ClockId(1), (0.0, 0.0).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap(); + let state = state + .add_clock(ClockId(2), (0.0, 0.0).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap(); + let state = state.add_link(LinkId(1), (0.0, 0.1).into()).unwrap(); + + let state = state + .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()))) + ); + } + + fn test_measure_external_clock_no_link() {} } diff --git a/statime-algo/src/matrix.rs b/statime-algo/src/matrix.rs index 4bc70cbe9..6c7e91f02 100644 --- a/statime-algo/src/matrix.rs +++ b/statime-algo/src/matrix.rs @@ -1,4 +1,4 @@ -use core::ops::{Add, AddAssign, Index, IndexMut, Mul, Sub, SubAssign}; +use core::ops::{Add, AddAssign, Div, Index, IndexMut, Mul, Sub, SubAssign}; /// A storage provider for a matrix. Abstracts a dynamically sized array of f64. /// @@ -164,6 +164,20 @@ impl Matrix { pub fn transpose(&self) -> Self { Matrix::new(self.cols, self.rows, |row, column| self[(column, row)]) } + + pub fn symmetrize(&self) -> Self { + // We can get away here without branching because floating point addition is + // symmetric. (IEEE 754, which is used in rust per the reference). + Matrix::new(self.rows, self.cols, |r, c| { + (self[(r, c)] + self[(c, r)]) / 2.0 + }) + } +} + +impl From for Matrix { + fn from(value: f64) -> Self { + Matrix::new(1, 1, |_, _| value) + } } impl Index<(usize, usize)> for Matrix { @@ -274,6 +288,20 @@ impl Mul for Matrix { } } +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, + storage: Storage::new(self.rows * self.cols, |index| lhs[index] / rhs), + } + } +} + impl Mul> for Matrix { type Output = Matrix; From 9c4649162808b5967c18e8c38e0a68d02389314f Mon Sep 17 00:00:00 2001 From: David Venhoek Date: Sun, 19 Jul 2026 13:23:26 +0200 Subject: [PATCH 05/18] Implemented external clocks and link delay uncertainty decay. --- statime-algo/src/estimator.rs | 137 +++++++++++++++++++++++++++++----- statime-algo/src/lib.rs | 2 +- 2 files changed, 119 insertions(+), 20 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 54c3fac7e..0611272f3 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -14,6 +14,7 @@ pub(crate) enum EstimatorError { ClockAlreadyExists, LinkNotFound, LinkAlreadyExists, + MeasurementBetweenExternalClocks, MatrixError(MatrixError), } @@ -60,6 +61,8 @@ impl ClockInfo { struct LinkInfo { id: LinkId, index: usize, + // Fraction of the link delay that we assume the error increases by every measurement + decay_rate: f64, } impl LinkInfo { @@ -67,6 +70,7 @@ impl LinkInfo { LinkInfo { id: self.id, index: new_index, + decay_rate: self.decay_rate, } } @@ -151,6 +155,11 @@ impl EstimatorState { delta_t * clock_info.wander.powi(2); } + for link_info in &self.link_info { + noise[(link_info.index, link_info.index)] = + delta_t * ((link_info.decay_rate * self.state[(link_info.index, 0)]).powi(2)); + } + EstimatorState { time: new_time, state: update.clone() * self.state.clone(), @@ -172,12 +181,24 @@ impl EstimatorState { offset: UncertainValue, link_delay: Option, ) -> Result { - let from_clock_info = self.get_clock_info(from)?; - let to_clock_info = self.get_clock_info(to)?; - let mut measurement_projection = Matrix::zero(1, self.state.rows()); - measurement_projection[(0, from_clock_info.offset_index())] = -1.0; - measurement_projection[(0, to_clock_info.offset_index())] = 1.0; + + 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)?; @@ -229,11 +250,41 @@ impl EstimatorState { /// Add an external clock to the estimator state. pub fn add_external_clock(&self, id: ClockId) -> Result { - todo!() + if self.clock_info.iter().any(|info| info.id == id) || self.external_clocks.contains(&id) { + return Err(EstimatorError::ClockAlreadyExists); + } + + let mut external_clocks = self.external_clocks.clone(); + external_clocks.push(id); + + Ok(EstimatorState { + time: self.time, + state: self.state.clone(), + uncertainty: self.uncertainty.clone(), + clock_info: self.clock_info.clone(), + external_clocks, + link_info: self.link_info.clone(), + }) } pub fn remove_external_clock(&self, id: ClockId) -> Result { - todo!() + if !self.external_clocks.contains(&id) { + return Err(EstimatorError::ClockAlreadyExists); + } + + Ok(EstimatorState { + time: self.time, + state: self.state.clone(), + uncertainty: self.uncertainty.clone(), + clock_info: self.clock_info.clone(), + external_clocks: self + .external_clocks + .iter() + .filter(|val| **val != id) + .copied() + .collect(), + link_info: self.link_info.clone(), + }) } /// Add a new clock to the estimator state.' @@ -299,10 +350,13 @@ impl EstimatorState { } /// Add a new link to the estimator state. + /// + /// The decay rate is the amount the uncertainty on the link delay increases every measurement on this link. pub fn add_link( &self, id: LinkId, initial_delay: UncertainValue, + decay_rate: f64, ) -> Result { if self.link_info.iter().any(|info| info.id == id) { return Err(EstimatorError::LinkAlreadyExists); @@ -311,6 +365,7 @@ impl EstimatorState { let new_link_info = LinkInfo { id, index: self.state.rows(), + decay_rate, }; let mut link_info = self.link_info.clone(); @@ -447,10 +502,11 @@ mod tests { let state = state .add_clock(ClockId(1), (0.0, 0.0).into(), (1e-6, 0.0).into(), 1e-8) .unwrap(); - let state = state.add_link(LinkId(1), (0.5, 0.2).into()).unwrap(); + let state = state.add_link(LinkId(1), (0.5, 0.2).into(), 0.0).unwrap(); let state = state .add_clock(ClockId(2), (0.0, 1e-5).into(), (-1e-6, 1e-7).into(), 0.0) .unwrap(); + let state = state.add_link(LinkId(2), (2.0, 0.0).into(), 0.1).unwrap(); let state = state.progress_time(100.0); assert_eq!(state.clock_frequency(ClockId(1)).unwrap().value, 1e-6); // Random walk noise, so frequency deviation is sqrt(time_interval)*wander. @@ -479,6 +535,11 @@ mod tests { 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] @@ -507,7 +568,7 @@ mod tests { fn test_add_link() { let state = EstimatorState::empty(0.0); let state = state - .add_link(LinkId(1), (1.0, 2.0).into()) + .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); @@ -595,7 +656,7 @@ mod tests { let state = state .add_clock(ClockId(2), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) .unwrap(); - let state = state.add_link(LinkId(1), (1.0, 0.0).into()).unwrap(); + let state = state.add_link(LinkId(1), (1.0, 0.0).into(), 0.0).unwrap(); let state = state .measurement( @@ -634,15 +695,10 @@ mod tests { let state = state .add_clock(ClockId(2), (0.0, 0.0).into(), (0.0, 1e-8).into(), 1e-8) .unwrap(); - let state = state.add_link(LinkId(1), (0.0, 0.1).into()).unwrap(); + let state = state.add_link(LinkId(1), (0.0, 0.1).into(), 0.0).unwrap(); let state = state - .measurement( - ClockId(1), - ClockId(2), - (1.0, 0.1).into(), - Some(LinkId(1)), - ) + .measurement(ClockId(1), ClockId(2), (1.0, 0.1).into(), Some(LinkId(1))) .unwrap(); assert_uv_almost_eq!( @@ -663,9 +719,52 @@ mod tests { ); assert_uv_almost_eq!( state.link_delay(LinkId(1)).unwrap(), - UncertainValue::from((0.5, 0.1/(2.0f64.sqrt()))) + UncertainValue::from((0.5, 0.1 / (2.0f64.sqrt()))) ); } - fn test_measure_external_clock_no_link() {} + #[test] + fn test_measure_external_clock_no_link() { + let state = EstimatorState::empty(0.0); + let state = state + .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap(); + let state = state.add_external_clock(ClockId(2)).unwrap(); + + let state = state + .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); + let state = state + .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) + .unwrap(); + let state = state.add_external_clock(ClockId(2)).unwrap(); + + let state = state + .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()); + } } diff --git a/statime-algo/src/lib.rs b/statime-algo/src/lib.rs index 52d64a26d..4360f2b0a 100644 --- a/statime-algo/src/lib.rs +++ b/statime-algo/src/lib.rs @@ -9,5 +9,5 @@ struct ClockId(u64); #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] struct LinkId(u64); -mod matrix; mod estimator; +mod matrix; From 0c50c39d5d522e51b3f353d52239fab00484876f Mon Sep 17 00:00:00 2001 From: David Venhoek Date: Sun, 19 Jul 2026 14:20:45 +0200 Subject: [PATCH 06/18] Allow math on matrix references. --- statime-algo/src/estimator.rs | 27 ++++--- statime-algo/src/matrix.rs | 136 ++++++++++++++++++++++++++++++---- 2 files changed, 135 insertions(+), 28 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 0611272f3..96cda1934 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -162,8 +162,8 @@ impl EstimatorState { EstimatorState { time: new_time, - state: update.clone() * self.state.clone(), - uncertainty: update.clone() * self.uncertainty.clone() * update.transpose() + noise, + state: &update * &self.state, + uncertainty: &update * &self.uncertainty * update.transpose() + noise, clock_info: self.clock_info.clone(), external_clocks: self.external_clocks.clone(), link_info: self.link_info.clone(), @@ -205,37 +205,36 @@ impl EstimatorState { measurement_projection[(0, link_delay_info.index)] = 1.0; } - let expected = measurement_projection.clone() * self.state.clone(); + 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.clone() - * self.uncertainty.clone() - * measurement_projection.transpose() - + offset.uncertainty.powi(2).into(); + 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.clone() * measurement_projection.transpose() - / difference_covariance[(0, 0)]; + let update_strength = + &self.uncertainty * measurement_projection.transpose() / difference_covariance[(0, 0)]; // This is simply using the strenght we calculated before to update the state - let new_state = self.state.clone() + update_strength.clone() * difference; + let new_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_proporitionality = - Matrix::identity(self.state.rows()) - update_strength.clone() * measurement_projection; - let new_uncertainty = (prev_step_proporitionality.clone() - * (self.uncertainty.clone() * prev_step_proporitionality.transpose()) - + update_strength.clone() * offset.uncertainty.powi(2) * update_strength.transpose()) + Matrix::identity(self.state.rows()) - &update_strength * measurement_projection; + let new_uncertainty = (&prev_step_proporitionality + * &self.uncertainty * prev_step_proporitionality.transpose() + + &update_strength * offset.uncertainty.powi(2) * update_strength.transpose()) .symmetrize(); Ok(EstimatorState { diff --git a/statime-algo/src/matrix.rs b/statime-algo/src/matrix.rs index 6c7e91f02..dbff3eb3a 100644 --- a/statime-algo/src/matrix.rs +++ b/statime-algo/src/matrix.rs @@ -198,10 +198,10 @@ impl IndexMut<(usize, usize)> for Matrix { } } -impl Add> for Matrix { +impl Add<&Matrix> for &Matrix { type Output = Matrix; - fn add(self, rhs: Matrix) -> Self::Output { + fn add(self, rhs: &Matrix) -> Self::Output { assert_eq!(self.cols, rhs.cols); assert_eq!(self.rows, rhs.rows); @@ -216,8 +216,32 @@ impl Add> for Matrix { } } -impl AddAssign> for Matrix { - fn add_assign(&mut self, rhs: Matrix) { +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(); @@ -228,10 +252,16 @@ impl AddAssign> for Matrix { } } -impl Sub> for Matrix { +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 { + fn sub(self, rhs: &Matrix) -> Self::Output { assert_eq!(self.cols, rhs.cols); assert_eq!(self.rows, rhs.rows); @@ -246,8 +276,32 @@ impl Sub> for Matrix { } } -impl SubAssign> for Matrix { - fn sub_assign(&mut self, rhs: Matrix) { +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(); @@ -258,10 +312,16 @@ impl SubAssign> for Matrix { } } -impl Mul> for f64 { +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 { + fn mul(self, rhs: &Matrix) -> Self::Output { let rows = rhs.rows; let cols = rhs.cols; let rhs = rhs.storage.as_ref(); @@ -274,7 +334,15 @@ impl Mul> for f64 { } } -impl Mul for Matrix { +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 { @@ -288,7 +356,15 @@ impl Mul for Matrix { } } -impl Div for Matrix { +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 { @@ -302,10 +378,18 @@ impl Div for Matrix { } } -impl Mul> for Matrix { +impl Div for Matrix { type Output = Matrix; - fn mul(self, rhs: Matrix) -> Self::Output { + 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(); @@ -325,6 +409,30 @@ impl Mul> for Matrix { } } +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")] From 92d15b748bd1d277e814f84d43048fdbd51e4a93 Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Mon, 20 Jul 2026 16:17:44 +0200 Subject: [PATCH 07/18] Take self instead of &self on mutating estimator functions --- statime-algo/src/estimator.rs | 459 ++++++++++++++++++---------------- statime-algo/src/lib.rs | 9 +- 2 files changed, 246 insertions(+), 222 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 96cda1934..e4c59ca86 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -1,4 +1,4 @@ -use std::{boxed::Box, sync::Arc, vec::Vec}; +use std::{boxed::Box, vec::Vec}; use crate::matrix::{Matrix, MatrixError}; @@ -8,13 +8,20 @@ use super::{ClockId, LinkId}; type Timestamp = f64; //FIXME: Make more permanent error enum +/// Errors that can occur when using the estimator. #[derive(Debug, Clone, PartialEq, Eq, Hash)] -pub(crate) enum EstimatorError { +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, + /// Error from the underlying matrix library MatrixError(MatrixError), } @@ -24,6 +31,40 @@ impl From for EstimatorError { } } +#[derive(Debug, Clone)] +struct ExternalClockList(Vec); + +impl ExternalClockList { + fn new() -> ExternalClockList { + ExternalClockList(Vec::new()) + } + + /// Returns true if the given clock is 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, @@ -39,21 +80,60 @@ impl ClockInfo { 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) + } - fn with_index(self, new_base_index: usize) -> ClockInfo { - ClockInfo { - id: self.id, - base_index: new_base_index, - wander: self.wander, + /// 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 self.0.iter_mut() { + if info.base_index > from { + info.base_index -= delta; + } } } - fn with_moved_index(self, from_index: usize, delta: usize) -> ClockInfo { - self.with_index(if self.base_index < from_index { - self.base_index + /// Remove the clock info for a given id, if it exists. + /// This updates the base indices of clocks where needed. + /// + /// Returns the removed clock info. + fn remove(&mut self, id: ClockId) -> Result { + let removed = if let Some(pos) = self.0.iter().position(|info| info.id == id) { + Ok(self.0.remove(pos)) } else { - self.base_index - delta - }) + Err(EstimatorError::ClockNotFound) + }?; + + self.update_indices(removed.base_index, 2); + + 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() } } @@ -65,37 +145,69 @@ struct LinkInfo { decay_rate: f64, } -impl LinkInfo { - fn with_index(self, new_index: usize) -> LinkInfo { - LinkInfo { - id: self.id, - index: new_index, - decay_rate: self.decay_rate, +#[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 self.0.iter_mut() { + if info.index > from { + info.index -= delta; + } } } - fn with_moved_index(self, from_index: usize, delta: usize) -> LinkInfo { - self.with_index(if self.index < from_index { - self.index + /// Remove the link info for a given id, if it exists. + /// This updates the indices for links where needed. + /// + /// Returns the removed clock info. + fn remove(&mut self, id: LinkId) -> Result { + let removed = if let Some(pos) = self.0.iter().position(|info| info.id == id) { + Ok(self.0.remove(pos)) } else { - self.index - delta - }) + Err(EstimatorError::LinkNotFound) + }?; + + self.update_indices(removed.index, 1); + + 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. #[derive(Debug, Clone)] -struct EstimatorState { +pub struct EstimatorState { time: Timestamp, state: Matrix>, uncertainty: Matrix>, - clock_info: Vec, - external_clocks: Vec, - link_info: Vec, + 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)] -struct UncertainValue { +pub struct UncertainValue { /// Best estimate of the value value: f64, /// Square root of the variance of the value. Corresponds @@ -122,21 +234,21 @@ impl EstimatorState { time, state: Matrix::zero(0, 1), uncertainty: Matrix::zero(0, 0), - clock_info: Vec::new(), - external_clocks: Vec::new(), - link_info: Vec::new(), + clock_info: ClockInfoList::new(), + external_clocks: ExternalClockList::new(), + link_info: LinkInfoList::new(), } } /// Progress the estimator state to the new timestamp. - pub fn progress_time(&self, new_time: Timestamp) -> EstimatorState { + pub fn progress_time(mut self, new_time: Timestamp) -> EstimatorState { 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 { + 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). @@ -155,19 +267,16 @@ impl EstimatorState { delta_t * clock_info.wander.powi(2); } - for link_info in &self.link_info { + 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)); } - EstimatorState { - time: new_time, - state: &update * &self.state, - uncertainty: &update * &self.uncertainty * update.transpose() + noise, - clock_info: self.clock_info.clone(), - external_clocks: self.external_clocks.clone(), - link_info: self.link_info.clone(), - } + self.time = new_time; + self.state = &update * &self.state; + self.uncertainty = &update * &self.uncertainty * update.transpose() + noise; + + self } /// Add a new measurement to the estimator state. @@ -175,7 +284,7 @@ impl EstimatorState { /// Assumes the measurements happens at the time the estimator state is /// currently set to. pub fn measurement( - &self, + mut self, from: ClockId, to: ClockId, offset: UncertainValue, @@ -183,8 +292,8 @@ impl EstimatorState { ) -> Result { 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); + 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); @@ -225,65 +334,39 @@ impl EstimatorState { &self.uncertainty * measurement_projection.transpose() / difference_covariance[(0, 0)]; // This is simply using the strenght we calculated before to update the state - let new_state = &self.state + &update_strength * difference; + 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_proporitionality = Matrix::identity(self.state.rows()) - &update_strength * measurement_projection; - let new_uncertainty = (&prev_step_proporitionality - * &self.uncertainty * prev_step_proporitionality.transpose() + self.uncertainty = (&prev_step_proporitionality + * &self.uncertainty + * prev_step_proporitionality.transpose() + &update_strength * offset.uncertainty.powi(2) * update_strength.transpose()) .symmetrize(); - Ok(EstimatorState { - time: self.time, - state: new_state, - uncertainty: new_uncertainty, - clock_info: self.clock_info.clone(), - external_clocks: self.external_clocks.clone(), - link_info: self.link_info.clone(), - }) + Ok(self) } /// Add an external clock to the estimator state. - pub fn add_external_clock(&self, id: ClockId) -> Result { - if self.clock_info.iter().any(|info| info.id == id) || self.external_clocks.contains(&id) { + 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); } - let mut external_clocks = self.external_clocks.clone(); - external_clocks.push(id); + self.external_clocks.add(id)?; - Ok(EstimatorState { - time: self.time, - state: self.state.clone(), - uncertainty: self.uncertainty.clone(), - clock_info: self.clock_info.clone(), - external_clocks, - link_info: self.link_info.clone(), - }) + Ok(self) } - pub fn remove_external_clock(&self, id: ClockId) -> Result { - if !self.external_clocks.contains(&id) { - return Err(EstimatorError::ClockAlreadyExists); - } + /// Remove an external clock from the estimator state. + pub fn remove_external_clock(mut self, id: ClockId) -> Result { + self.external_clocks.remove(id)?; - Ok(EstimatorState { - time: self.time, - state: self.state.clone(), - uncertainty: self.uncertainty.clone(), - clock_info: self.clock_info.clone(), - external_clocks: self - .external_clocks - .iter() - .filter(|val| **val != id) - .copied() - .collect(), - link_info: self.link_info.clone(), - }) + Ok(self) } /// Add a new clock to the estimator state.' @@ -291,13 +374,14 @@ impl EstimatorState { /// To add a new clock you must provide the initial values for the offset, /// frequency and wander of the clock. pub fn add_clock( - &self, + mut self, id: ClockId, initial_offset: UncertainValue, initial_frequency: UncertainValue, initial_wander: f64, ) -> Result { - if self.clock_info.iter().any(|info| info.id == id) { + // check in external clocks as well + if self.external_clocks.contains(id) { return Err(EstimatorError::ClockAlreadyExists); } @@ -307,102 +391,61 @@ impl EstimatorState { wander: initial_wander, }; - let mut clock_info = self.clock_info.clone(); - clock_info.push(new_clock_info); - - Ok(EstimatorState { - time: self.time, - state: self - .state - .extend_vec([initial_offset.value, initial_frequency.value])?, - uncertainty: self.uncertainty.extend([ - [initial_offset.uncertainty.powi(2), 0.0], - [0.0, initial_frequency.uncertainty.powi(2)], - ]), - clock_info, - external_clocks: self.external_clocks.clone(), - link_info: self.link_info.clone(), - }) + 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(&self, id: ClockId) -> Result { - let clock_info = self.get_clock_info(id)?; + pub fn remove_clock(mut self, id: ClockId) -> Result { + let clock_info = self.clock_info.remove(id)?; - Ok(EstimatorState { - time: self.time, - state: self.state.splice_vec(clock_info.base_index, 2)?, - uncertainty: self.uncertainty.splice_square(clock_info.base_index, 2)?, - clock_info: self - .clock_info - .iter() - .filter(|info| info.id != id) - .map(|info| info.with_moved_index(clock_info.base_index, 2)) - .collect(), - external_clocks: self.external_clocks.clone(), - link_info: self - .link_info - .iter() - .map(|link_info| link_info.with_moved_index(clock_info.base_index, 2)) - .collect(), - }) + self.state = self.state.splice_vec(clock_info.base_index, 2)?; + self.uncertainty = self.uncertainty.splice_square(clock_info.base_index, 2)?; + self.link_info.update_indices(clock_info.base_index, 2); + + Ok(self) } /// Add a new link to the estimator state. /// /// The decay rate is the amount the uncertainty on the link delay increases every measurement on this link. pub fn add_link( - &self, + mut self, id: LinkId, initial_delay: UncertainValue, decay_rate: f64, ) -> Result { - if self.link_info.iter().any(|info| info.id == id) { - return Err(EstimatorError::LinkAlreadyExists); - } - let new_link_info = LinkInfo { id, index: self.state.rows(), decay_rate, }; - let mut link_info = self.link_info.clone(); - link_info.push(new_link_info); - - Ok(EstimatorState { - time: self.time, - state: self.state.extend_vec([initial_delay.value])?, - uncertainty: self - .uncertainty - .extend([[initial_delay.uncertainty.powi(2)]]), - clock_info: self.clock_info.clone(), - external_clocks: self.external_clocks.clone(), - link_info, - }) + 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(&self, id: LinkId) -> Result { - let link_info = self.get_link_info(id)?; + pub fn remove_link(mut self, id: LinkId) -> Result { + let removed_info = self.link_info.remove(id)?; + self.state = self.state.splice_vec(removed_info.index, 1)?; + self.uncertainty = self.uncertainty.splice_square(removed_info.index, 1)?; + self.clock_info.update_indices(removed_info.index, 1); - Ok(EstimatorState { - time: self.time, - state: self.state.splice_vec(link_info.index, 1)?, - uncertainty: self.uncertainty.splice_square(link_info.index, 1)?, - clock_info: self - .clock_info - .iter() - .map(|clock_info| clock_info.with_moved_index(link_info.index, 1)) - .collect(), - external_clocks: self.external_clocks.clone(), - link_info: self - .link_info - .iter() - .filter(|info| info.id != id) - .map(|info| info.with_moved_index(link_info.index, 1)) - .collect(), - }) + Ok(self) } /// Get the current offset of a clock in the state, along with the uncertainty of that offset. @@ -485,8 +528,7 @@ mod tests { #[test] fn test_add_clock() { - let state = EstimatorState::empty(0.0); - let state = state + 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); @@ -497,16 +539,16 @@ mod tests { #[test] fn test_time_evolve() { - let state = EstimatorState::empty(0.0); - let state = state + 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 = state.add_link(LinkId(1), (0.5, 0.2).into(), 0.0).unwrap(); - let state = state + .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(); - let state = state.add_link(LinkId(2), (2.0, 0.0).into(), 0.1).unwrap(); - let state = state.progress_time(100.0); + .unwrap() + .add_link(LinkId(2), (2.0, 0.0).into(), 0.1) + .unwrap() + .progress_time(100.0); 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); @@ -543,16 +585,13 @@ mod tests { #[test] fn test_progress_time_composes_well() { - let state = EstimatorState::empty(0.0); - let state = state + 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).progress_time(100.0); let state_at_once = state.progress_time(100.0); - let state_intermediate = state.progress_time(75.0); - let state_via_intermediate = state_intermediate.progress_time(100.0); - assert_uv_almost_eq!( state_at_once.clock_offset(ClockId(1)).unwrap(), state_via_intermediate.clock_offset(ClockId(1)).unwrap() @@ -565,8 +604,7 @@ mod tests { #[test] fn test_add_link() { - let state = EstimatorState::empty(0.0); - let state = state + 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); @@ -575,15 +613,11 @@ mod tests { #[test] fn test_measure_between_clocks_no_link() { - let state = EstimatorState::empty(0.0); - let state = state + let state = EstimatorState::empty(0.0) .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) - .unwrap(); - let state = state + .unwrap() .add_clock(ClockId(2), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) - .unwrap(); - - let state = state + .unwrap() .measurement( ClockId(1), ClockId(2), @@ -609,17 +643,12 @@ mod tests { UncertainValue::from((0.0, 1e-8)) ); - let state = EstimatorState::empty(0.0); - let state = state + let state = EstimatorState::empty(0.0) .add_clock(ClockId(1), (0.0, 0.0).into(), (0.0, 1e-3).into(), 0.0) - .unwrap(); - let state = state + .unwrap() .add_clock(ClockId(2), (0.0, 0.0).into(), (0.0, 1e-3).into(), 0.0) - .unwrap(); - - let state = state.progress_time(100.0); - - let state = state + .unwrap() + .progress_time(100.0) .measurement( ClockId(1), ClockId(2), @@ -648,16 +677,13 @@ mod tests { #[test] fn test_measure_between_clocks_with_link() { - let state = EstimatorState::empty(0.0); - let state = state + let state = EstimatorState::empty(0.0) .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) - .unwrap(); - let state = state + .unwrap() .add_clock(ClockId(2), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) - .unwrap(); - let state = state.add_link(LinkId(1), (1.0, 0.0).into(), 0.0).unwrap(); - - let state = state + .unwrap() + .add_link(LinkId(1), (1.0, 0.0).into(), 0.0) + .unwrap() .measurement( ClockId(1), ClockId(2), @@ -687,16 +713,13 @@ mod tests { UncertainValue::from((1.0, 0.0)) ); - let state = EstimatorState::empty(0.0); - let state = state + let state = EstimatorState::empty(0.0) .add_clock(ClockId(1), (0.0, 0.0).into(), (0.0, 1e-8).into(), 1e-8) - .unwrap(); - let state = state + .unwrap() .add_clock(ClockId(2), (0.0, 0.0).into(), (0.0, 1e-8).into(), 1e-8) - .unwrap(); - let state = state.add_link(LinkId(1), (0.0, 0.1).into(), 0.0).unwrap(); - - let state = state + .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(); @@ -724,13 +747,11 @@ mod tests { #[test] fn test_measure_external_clock_no_link() { - let state = EstimatorState::empty(0.0); - let state = state + let state = EstimatorState::empty(0.0) .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) - .unwrap(); - let state = state.add_external_clock(ClockId(2)).unwrap(); - - let state = state + .unwrap() + .add_external_clock(ClockId(2)) + .unwrap() .measurement(ClockId(2), ClockId(1), (1.0, 0.1).into(), None) .unwrap(); @@ -744,13 +765,11 @@ mod tests { UncertainValue::from((0.0, 1e-8)) ); - let state = EstimatorState::empty(0.0); - let state = state + let state = EstimatorState::empty(0.0) .add_clock(ClockId(1), (0.0, 0.1).into(), (0.0, 1e-8).into(), 1e-8) - .unwrap(); - let state = state.add_external_clock(ClockId(2)).unwrap(); - - let state = state + .unwrap() + .add_external_clock(ClockId(2)) + .unwrap() .measurement(ClockId(1), ClockId(2), (1.0, 0.1).into(), None) .unwrap(); diff --git a/statime-algo/src/lib.rs b/statime-algo/src/lib.rs index 4360f2b0a..7419ab1e6 100644 --- a/statime-algo/src/lib.rs +++ b/statime-algo/src/lib.rs @@ -4,10 +4,15 @@ #[cfg(feature = "std")] extern crate std; +/// TODO: replace #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] -struct ClockId(u64); +pub struct ClockId(u64); + +/// TODO: replace #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] -struct LinkId(u64); +pub struct LinkId(u64); mod estimator; mod matrix; + +pub use estimator::{EstimatorError, EstimatorState}; From dc4459526d449f1a0db702cdd8c726c56b8e47c7 Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Mon, 20 Jul 2026 16:27:00 +0200 Subject: [PATCH 08/18] Add test for clock removal --- statime-algo/src/estimator.rs | 39 +++++++++++++++++++++++++++++++---- 1 file changed, 35 insertions(+), 4 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index e4c59ca86..2bd1e6a59 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -497,10 +497,7 @@ impl EstimatorState { #[cfg(test)] mod tests { - use crate::{ - ClockId, LinkId, - estimator::{EstimatorState, UncertainValue}, - }; + use super::*; macro_rules! assert_almost_eq { ($left:expr, $right:expr) => { @@ -537,6 +534,40 @@ mod tests { assert_eq!(state.clock_frequency(ClockId(1)).unwrap().uncertainty, 3.0); } + #[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) From 039f0b4ecc3155d76a648fb79a8eef1b0c7f1e00 Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Mon, 20 Jul 2026 17:00:25 +0200 Subject: [PATCH 09/18] Add some more tests --- statime-algo/src/estimator.rs | 58 +++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 2bd1e6a59..f9cd75cf4 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -532,6 +532,19 @@ mod tests { 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] @@ -816,4 +829,49 @@ mod tests { assert!(state.remove_external_clock(ClockId(2)).is_ok()); } + + #[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 + ); + } } From d87a085ba8946a4370f408bf8b4c4e3ce2a4f4b2 Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Mon, 20 Jul 2026 17:12:59 +0200 Subject: [PATCH 10/18] Fix some typos, expose UncertainValue fields --- statime-algo/src/estimator.rs | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index f9cd75cf4..80fe43814 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -39,7 +39,7 @@ impl ExternalClockList { ExternalClockList(Vec::new()) } - /// Returns true if the given clock is is known as an external clock. + /// Returns true if the given clock is known as an external clock. fn contains(&self, id: ClockId) -> bool { self.0.contains(&id) } @@ -165,7 +165,7 @@ impl LinkInfoList { /// Remove the link info for a given id, if it exists. /// This updates the indices for links where needed. /// - /// Returns the removed clock info. + /// Returns the removed link info. fn remove(&mut self, id: LinkId) -> Result { let removed = if let Some(pos) = self.0.iter().position(|info| info.id == id) { Ok(self.0.remove(pos)) @@ -209,10 +209,10 @@ pub struct EstimatorState { #[derive(Debug, Clone, Copy, PartialEq)] pub struct UncertainValue { /// Best estimate of the value - value: f64, + pub value: f64, /// Square root of the variance of the value. Corresponds /// to 1 standard deviation. - uncertainty: f64, + pub uncertainty: f64, } /// Convert from a tuple of (value, uncertainty) to an `UncertainValue`. @@ -333,17 +333,17 @@ impl EstimatorState { let update_strength = &self.uncertainty * measurement_projection.transpose() / difference_covariance[(0, 0)]; - // This is simply using the strenght we calculated before to update the state + // 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_proporitionality = + let prev_step_proportionality = Matrix::identity(self.state.rows()) - &update_strength * measurement_projection; - self.uncertainty = (&prev_step_proporitionality + self.uncertainty = (&prev_step_proportionality * &self.uncertainty - * prev_step_proporitionality.transpose() + * prev_step_proportionality.transpose() + &update_strength * offset.uncertainty.powi(2) * update_strength.transpose()) .symmetrize(); @@ -458,7 +458,7 @@ impl EstimatorState { }) } - /// Get the current freqency of a clock in the state, along with the uncertainty of that frequency. + /// 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 { From a64854cf0c1d7fdd2a56cdf439af232c62dc2ef4 Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Mon, 20 Jul 2026 17:34:29 +0200 Subject: [PATCH 11/18] Add some assumtion docs, add some additional checks --- statime-algo/src/estimator.rs | 59 ++++++++++++++++++++++++++++++++--- 1 file changed, 54 insertions(+), 5 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 80fe43814..85fb40f4f 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -21,6 +21,10 @@ pub enum EstimatorError { 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), } @@ -195,6 +199,13 @@ impl LinkInfoList { } /// 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, @@ -241,7 +252,17 @@ impl EstimatorState { } /// Progress the estimator state to the new timestamp. - pub fn progress_time(mut self, new_time: Timestamp) -> EstimatorState { + 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 + if new_time == self.time { + return Ok(self); + } + let delta_t = new_time - self.time; let mut update = Matrix::identity(self.state.rows()); @@ -276,7 +297,7 @@ impl EstimatorState { self.state = &update * &self.state; self.uncertainty = &update * &self.uncertainty * update.transpose() + noise; - self + Ok(self) } /// Add a new measurement to the estimator state. @@ -290,6 +311,10 @@ impl EstimatorState { 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); @@ -592,7 +617,8 @@ mod tests { .unwrap() .add_link(LinkId(2), (2.0, 0.0).into(), 0.1) .unwrap() - .progress_time(100.0); + .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); @@ -633,8 +659,13 @@ mod tests { .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).progress_time(100.0); - let state_at_once = state.progress_time(100.0); + 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(), @@ -693,6 +724,7 @@ mod tests { .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), @@ -830,6 +862,15 @@ mod tests { 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) @@ -873,5 +914,13 @@ mod tests { .unwrap_err(), EstimatorError::LinkNotFound ); + + assert_eq!( + state + .clone() + .measurement(ClockId(3), ClockId(3), (0.0, 0.1).into(), None) + .unwrap_err(), + EstimatorError::MeasurementBetweenSelf + ); } } From b1a327b9371c4a74cf3e5c8c1f7db79135b2f5d5 Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Mon, 20 Jul 2026 21:55:30 +0200 Subject: [PATCH 12/18] Ensure symmetrize checks against square matrices --- statime-algo/src/estimator.rs | 2 +- statime-algo/src/matrix.rs | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 85fb40f4f..8d881266d 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -370,7 +370,7 @@ impl EstimatorState { * &self.uncertainty * prev_step_proportionality.transpose() + &update_strength * offset.uncertainty.powi(2) * update_strength.transpose()) - .symmetrize(); + .symmetrize()?; Ok(self) } diff --git a/statime-algo/src/matrix.rs b/statime-algo/src/matrix.rs index dbff3eb3a..597f1d0ec 100644 --- a/statime-algo/src/matrix.rs +++ b/statime-algo/src/matrix.rs @@ -165,12 +165,16 @@ impl Matrix { Matrix::new(self.cols, self.rows, |row, column| self[(column, row)]) } - pub fn symmetrize(&self) -> Self { + 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). - Matrix::new(self.rows, self.cols, |r, c| { + Ok(Matrix::new(self.rows, self.cols, |r, c| { (self[(r, c)] + self[(c, r)]) / 2.0 - }) + })) } } From fd3a292e6108b746c2f9e47bc9b5f21444fb9703 Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Mon, 20 Jul 2026 21:55:49 +0200 Subject: [PATCH 13/18] Fix comment --- statime-algo/src/estimator.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 8d881266d..d1c881a7b 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -145,7 +145,7 @@ impl ClockInfoList { struct LinkInfo { id: LinkId, index: usize, - // Fraction of the link delay that we assume the error increases by every measurement + /// Fraction of the link delay that we assume the error increases by every second decay_rate: f64, } From 91f1bc9979d4fbad4565a5b3a48732df62a718bc Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Mon, 20 Jul 2026 22:50:42 +0200 Subject: [PATCH 14/18] Better enforcement of removal of links and clocks --- statime-algo/src/estimator.rs | 50 +++++++++++++++++++++++++---------- 1 file changed, 36 insertions(+), 14 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index d1c881a7b..081f9a5c7 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -77,6 +77,8 @@ struct ClockInfo { } impl ClockInfo { + const SIZE: usize = 2; + fn offset_index(self) -> usize { self.base_index } @@ -110,17 +112,23 @@ impl ClockInfoList { } /// Remove the clock info for a given id, if it exists. - /// This updates the base indices of clocks where needed. + /// + /// Updates the indices for clocks and links where needed. /// /// Returns the removed clock info. - fn remove(&mut self, id: ClockId) -> Result { + 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, 2); + self.update_indices(removed.base_index, ClockInfo::SIZE); + link_info.update_indices(removed.base_index, ClockInfo::SIZE); Ok(removed) } @@ -149,6 +157,10 @@ struct LinkInfo { decay_rate: f64, } +impl LinkInfo { + const SIZE: usize = 1; +} + #[derive(Debug, Clone)] struct LinkInfoList(Vec); @@ -167,17 +179,23 @@ impl LinkInfoList { } /// Remove the link info for a given id, if it exists. - /// This updates the indices for links where needed. + /// + /// Updates the indices for links and clocks where needed. /// /// Returns the removed link info. - fn remove(&mut self, id: LinkId) -> Result { + 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, 1); + self.update_indices(removed.index, LinkInfo::SIZE); + clock_info.update_indices(removed.index, LinkInfo::SIZE); Ok(removed) } @@ -430,11 +448,14 @@ impl EstimatorState { /// Remove a clock from the estimator state. pub fn remove_clock(mut self, id: ClockId) -> Result { - let clock_info = self.clock_info.remove(id)?; + let clock_info = self.clock_info.remove(id, &mut self.link_info)?; - self.state = self.state.splice_vec(clock_info.base_index, 2)?; - self.uncertainty = self.uncertainty.splice_square(clock_info.base_index, 2)?; - self.link_info.update_indices(clock_info.base_index, 2); + 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) } @@ -465,10 +486,11 @@ impl EstimatorState { /// Remove a link from the estimator state. pub fn remove_link(mut self, id: LinkId) -> Result { - let removed_info = self.link_info.remove(id)?; - self.state = self.state.splice_vec(removed_info.index, 1)?; - self.uncertainty = self.uncertainty.splice_square(removed_info.index, 1)?; - self.clock_info.update_indices(removed_info.index, 1); + 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) } From de916a3a34c40663c131448e7bae6f92f4c3f0a2 Mon Sep 17 00:00:00 2001 From: Ruben Nijveld Date: Mon, 20 Jul 2026 23:05:17 +0200 Subject: [PATCH 15/18] Clean up to get rid of some clippy lint warnings --- statime-algo/src/estimator.rs | 8 +++++--- statime-algo/src/matrix.rs | 6 +++++- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 081f9a5c7..d59ebc374 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -104,7 +104,7 @@ impl ClockInfoList { /// 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 self.0.iter_mut() { + for info in &mut self.0 { if info.base_index > from { info.base_index -= delta; } @@ -171,7 +171,7 @@ impl LinkInfoList { /// 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 self.0.iter_mut() { + for info in &mut self.0 { if info.index > from { info.index -= delta; } @@ -258,6 +258,7 @@ 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, @@ -277,6 +278,7 @@ impl EstimatorState { } // 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); } @@ -462,7 +464,7 @@ impl EstimatorState { /// Add a new link to the estimator state. /// - /// The decay rate is the amount the uncertainty on the link delay increases every measurement on this link. + /// 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, diff --git a/statime-algo/src/matrix.rs b/statime-algo/src/matrix.rs index 597f1d0ec..03d89c993 100644 --- a/statime-algo/src/matrix.rs +++ b/statime-algo/src/matrix.rs @@ -173,7 +173,7 @@ impl Matrix { // 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| { - (self[(r, c)] + self[(c, r)]) / 2.0 + f64::midpoint(self[(r, c)], self[(c, r)]) })) } } @@ -377,6 +377,10 @@ impl Div for &Matrix { 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), } } From ff1b1628eaec06ec14a1d15ae81c6d8c8784b82d Mon Sep 17 00:00:00 2001 From: David Venhoek Date: Tue, 21 Jul 2026 14:42:34 +0200 Subject: [PATCH 16/18] Implement estimation of link noise. --- statime-algo/src/lib.rs | 16 +++ statime-algo/src/link_noise.rs | 234 +++++++++++++++++++++++++++++++++ 2 files changed, 250 insertions(+) create mode 100644 statime-algo/src/link_noise.rs diff --git a/statime-algo/src/lib.rs b/statime-algo/src/lib.rs index 7419ab1e6..0fdb5a1c1 100644 --- a/statime-algo/src/lib.rs +++ b/statime-algo/src/lib.rs @@ -12,7 +12,23 @@ pub struct ClockId(u64); #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] pub struct LinkId(u64); +#[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 link_noise; mod matrix; 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..d658acc4b --- /dev/null +++ b/statime-algo/src/link_noise.rs @@ -0,0 +1,234 @@ +use crate::{ + ClockId, LinkNoiseError::NotEnoughMeasurements, link_noise::LinkNoiseError::InvalidClocks, +}; + +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, Clone, PartialEq, Default)] +struct UnorderedRingBuffer { + values: [f64; DELAYS], + n_values: usize, + write_idx: usize, +} + +impl UnorderedRingBuffer { + fn insert(&mut self, value: f64) { + self.values[self.write_idx] = value; + self.write_idx = (self.write_idx + 1) % DELAYS; + self.n_values = (self.n_values + 1).min(DELAYS); + } +} + +impl AsRef<[f64]> for UnorderedRingBuffer { + fn as_ref(&self) -> &[f64] { + &self.values[..self.n_values] + } +} + +#[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); + } +} From 8519ee84d964ff38da1929473c86f57c95bab5f5 Mon Sep 17 00:00:00 2001 From: David Venhoek Date: Wed, 22 Jul 2026 10:12:06 +0200 Subject: [PATCH 17/18] Add processing of frequency steering. --- statime-algo/src/estimator.rs | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index d59ebc374..7b8a3415c 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -320,6 +320,20 @@ impl EstimatorState { 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 @@ -677,6 +691,20 @@ mod tests { ); } + #[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) From ba809c9454e92a65de34088424b64e211175c1eb Mon Sep 17 00:00:00 2001 From: David Venhoek Date: Thu, 23 Jul 2026 09:29:37 +0200 Subject: [PATCH 18/18] Initial work on agreement filter. --- statime-algo/src/estimator.rs | 10 ++ statime-algo/src/filter.rs | 198 +++++++++++++++++++++++++++++++++ statime-algo/src/lib.rs | 34 +++++- statime-algo/src/link_noise.rs | 23 +--- statime-algo/src/ringbuffer.rs | 22 ++++ 5 files changed, 259 insertions(+), 28 deletions(-) create mode 100644 statime-algo/src/filter.rs create mode 100644 statime-algo/src/ringbuffer.rs diff --git a/statime-algo/src/estimator.rs b/statime-algo/src/estimator.rs index 7b8a3415c..08b52076b 100644 --- a/statime-algo/src/estimator.rs +++ b/statime-algo/src/estimator.rs @@ -540,6 +540,16 @@ impl EstimatorState { 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 { 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 index 0fdb5a1c1..52114dcc5 100644 --- a/statime-algo/src/lib.rs +++ b/statime-algo/src/lib.rs @@ -4,13 +4,31 @@ #[cfg(feature = "std")] extern crate std; -/// TODO: replace -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] -pub struct ClockId(u64); +/// 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); -/// TODO: replace -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] -pub struct LinkId(u64); +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 { @@ -27,8 +45,12 @@ macro_rules! assert_almost_eq { } 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 index d658acc4b..2fccc619f 100644 --- a/statime-algo/src/link_noise.rs +++ b/statime-algo/src/link_noise.rs @@ -1,5 +1,5 @@ use crate::{ - ClockId, LinkNoiseError::NotEnoughMeasurements, link_noise::LinkNoiseError::InvalidClocks, + ClockId, LinkNoiseError::NotEnoughMeasurements, link_noise::LinkNoiseError::InvalidClocks, ringbuffer::UnorderedRingBuffer, }; const DELAYS: usize = 8; @@ -9,27 +9,6 @@ const MAX_TIME_BETWEEN_HALVES: f64 = 0.5; type Timestamp = f64; -#[derive(Debug, Clone, PartialEq, Default)] -struct UnorderedRingBuffer { - values: [f64; DELAYS], - n_values: usize, - write_idx: usize, -} - -impl UnorderedRingBuffer { - fn insert(&mut self, value: f64) { - self.values[self.write_idx] = value; - self.write_idx = (self.write_idx + 1) % DELAYS; - self.n_values = (self.n_values + 1).min(DELAYS); - } -} - -impl AsRef<[f64]> for UnorderedRingBuffer { - fn as_ref(&self) -> &[f64] { - &self.values[..self.n_values] - } -} - #[derive(Debug, Copy, Clone, PartialEq)] struct PreviousMeasurement { time: Timestamp, 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] + } +}