diff --git a/statime/src/filters/fixed_wander.rs b/statime/src/filters/fixed_wander.rs new file mode 100644 index 000000000..b8f0b6bff --- /dev/null +++ b/statime/src/filters/fixed_wander.rs @@ -0,0 +1,597 @@ +//! A compact Kalman clock servo for systems with a characterized oscillator. +//! +//! Unlike [`super::KalmanFilter`], this filter uses one estimator and a fixed, +//! configured oscillator-wander model. It still estimates network measurement +//! noise online. This removes the second estimator and its convergence state, +//! but makes the quality of `frequency_wander` part of the application tuning. +//! +//! The [noise-estimation design used by Statime][paper] needs its second, +//! temporarily open-loop estimator specifically because network noise obscures +//! oscillator wander at short intervals. Omitting that estimator is justified +//! when oscillator wander has instead been characterized or conservatively +//! bounded for the target and environment; it is not a generally equivalent +//! replacement for online wander estimation. +//! +//! [paper]: https://tweedegolf.nl/images/estimating-noise-for-clock-synchronizing-kalman-filters-copyright.pdf + +use super::{kalman::InnerFilter, Filter, FilterEstimate, FilterUpdate}; +use crate::{ + port::Measurement, + time::{Duration, Time}, + Clock, +}; + +const ERROR_SAMPLES: usize = 32; + +// Match f64::clamp without pulling float formatting into embedded panic paths. +#[allow(clippy::manual_clamp)] +fn clamp(mut value: f64, min: f64, max: f64) -> f64 { + assert!(min <= max, "invalid clamp bounds"); + if value < min { + value = min; + } + if value > max { + value = max; + } + value +} + +fn sqr(value: f64) -> f64 { + value * value +} + +/// Configuration for [`FixedWanderKalmanFilter`]. +/// +/// The defaults are a reference preset for an embedded ordinary clock with an +/// uncompensated crystal oscillator, hardware packet timestamps, and one-Hz +/// Sync. They deliberately favor conservative startup over trusting the first +/// few network observations. Applications with oscillator characterization +/// should override `frequency_wander`. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct FixedWanderKalmanConfig { + /// Offset above which the clock is stepped instead of slewed. + pub step_threshold: Duration, + /// Time over which an estimated offset is removed by frequency steering. + pub steer_time: Duration, + /// Maximum phase-removal frequency correction, in ppm. + pub max_steer: f64, + /// Maximum total clock frequency correction, in ppm. + pub max_frequency: f64, + /// Initial one-sigma fractional-frequency uncertainty. + /// + /// The 100 ppm default covers the initial tolerance of common XOs without + /// immediately saturating the default clock-actuator range. + pub initial_frequency_uncertainty: f64, + /// Initial one-sigma timestamp-measurement uncertainty. + /// + /// This conservative value is used until four closely paired Sync and + /// DelayReq observations allow measurement noise to be estimated online. + pub initial_measurement_uncertainty: Duration, + /// Fractional-frequency random-walk variance per second. + /// + /// This is the `A` term in the process covariance. In the cited paper, an + /// Intel I210 oscillator measured independently and estimated online under + /// good network conditions both gave approximately `6.25e-18`. The default + /// is the next, four-times-larger estimator bin as a conservative reference; + /// it is not a substitute for target and environment characterization. + pub frequency_wander: f64, + /// Relative path-delay random-walk variance per second. + /// + /// The default grows an initially exact delay estimate to one-percent + /// standard uncertainty after one hour without further observations. + pub delay_wander: f64, +} + +impl Default for FixedWanderKalmanConfig { + fn default() -> Self { + Self { + step_threshold: Duration::from_seconds(1e-3), + steer_time: Duration::from_seconds(2.0), + max_steer: 200.0, + max_frequency: 400.0, + initial_frequency_uncertainty: 100e-6, + initial_measurement_uncertainty: Duration::from_seconds(1e-3), + frequency_wander: 2.5e-17, + delay_wander: 1e-4 / 3600.0, + } + } +} + +#[derive(Clone, Copy, Debug, Default)] +struct MeasurementNoise { + data: [f64; ERROR_SAMPLES], + next: usize, + len: usize, + last_sync: Option<(Time, Duration)>, + last_delay: Option<(Time, Duration)>, + peer_delay: bool, +} + +impl MeasurementNoise { + const RANGE_SAMPLES: usize = 4; + const VARIANCE_SAMPLES: usize = 8; + + fn observe(&mut self, measurement: Measurement, frequency: f64) { + if let Some(sync) = measurement.raw_sync_offset { + if let Some((time, delay)) = self.last_delay.take() { + if (measurement.event_time - time).abs() < Duration::from_millis(200) { + self.push( + sync.seconds() - delay.seconds() + + (time - measurement.event_time).seconds() * frequency, + ); + } else { + self.last_sync = Some((measurement.event_time, sync)); + } + } else { + self.last_sync = Some((measurement.event_time, sync)); + } + } + + if let Some(delay) = measurement.raw_delay_offset { + if let Some((time, sync)) = self.last_sync.take() { + if (measurement.event_time - time).abs() < Duration::from_millis(200) { + self.push( + sync.seconds() - delay.seconds() + + (measurement.event_time - time).seconds() * frequency, + ); + } else { + self.last_delay = Some((measurement.event_time, delay)); + } + } else { + self.last_delay = Some((measurement.event_time, delay)); + } + } + + if let Some(delay) = measurement.peer_delay { + self.last_sync = None; + self.last_delay = None; + self.peer_delay = true; + self.push(delay.seconds()); + } + } + + fn push(&mut self, value: f64) { + self.data[self.next] = value; + self.next = (self.next + 1) % self.data.len(); + self.len = (self.len + 1).min(self.data.len()); + } + + fn variance(&self, config: &FixedWanderKalmanConfig) -> f64 { + if self.len < Self::RANGE_SAMPLES { + sqr(config.initial_measurement_uncertainty.seconds()) + } else if self.len < Self::VARIANCE_SAMPLES { + let values = &self.data[..self.len]; + let (min, max) = values + .iter() + .copied() + .fold((f64::INFINITY, f64::NEG_INFINITY), |(min, max), value| { + (min.min(value), max.max(value)) + }); + sqr(max - min) + } else { + let values = &self.data[..self.len]; + let mean = values.iter().sum::() / self.len as f64; + // Sync-minus-DelayReq contains two independent one-way errors, so + // its sample variance is twice either observation's variance. + values.iter().map(|value| sqr(value - mean)).sum::() + / (2.0 * (self.len - 1) as f64) + } + } +} + +/// Three-state Kalman clock servo with fixed oscillator-wander covariance. +/// +/// The filter estimates local-minus-master phase, residual fractional +/// frequency, and mean path delay. It uses one estimator and specialized scalar +/// algebra, making it smaller than [`super::KalmanFilter`], which runs a second +/// estimator to learn oscillator wander. +/// +/// Use this filter when the oscillator and its operating environment are known +/// well enough to configure [`FixedWanderKalmanConfig::frequency_wander`], or +/// when deterministic memory, code size, and startup behavior matter more than +/// adapting across unknown hardware. A poor wander value can make the filter +/// either sluggish and overconfident (too small) or noisy (too large); prefer +/// [`super::KalmanFilter`] for general-purpose systems without that knowledge. +pub struct FixedWanderKalmanFilter { + config: FixedWanderKalmanConfig, + estimate: Option, + noise: MeasurementNoise, + frequency: Option, +} + +impl Filter for FixedWanderKalmanFilter { + type Config = FixedWanderKalmanConfig; + + fn new(config: Self::Config) -> Self { + Self { + config, + estimate: None, + noise: MeasurementNoise::default(), + frequency: None, + } + } + + fn measurement(&mut self, measurement: Measurement, clock: &mut C) -> FilterUpdate { + if let Some(estimate) = self.estimate.as_ref() { + if measurement.event_time < estimate.time() { + return FilterUpdate::default(); + } + } + + self.noise.observe( + measurement, + self.estimate.as_ref().map_or(0.0, InnerFilter::frequency), + ); + let variance = + self.noise.variance(&self.config) * if self.noise.peer_delay { 2.0 } else { 1.0 }; + + if measurement.raw_sync_offset.is_some() || measurement.raw_delay_offset.is_some() { + self.ensure_frequency(clock); + } + + let estimate = self.estimate.get_or_insert_with(|| { + InnerFilter::new( + 0.0, + measurement.event_time, + self.config.step_threshold, + self.config.initial_frequency_uncertainty, + ) + }); + estimate.progress_filtertime( + measurement.event_time, + self.config.frequency_wander, + self.config.delay_wander, + ); + if let Some(value) = measurement.raw_sync_offset { + let value = value.seconds(); + if (value - estimate.offset()).abs() > self.config.step_threshold.seconds() { + *estimate = InnerFilter::new( + value, + estimate.time(), + self.config.step_threshold, + self.config.initial_frequency_uncertainty, + ); + } else { + estimate.absorb_sync_offset(value, variance); + } + } + if let Some(value) = measurement.raw_delay_offset { + let value = value.seconds(); + if (value - estimate.offset()).abs() > self.config.step_threshold.seconds() { + *estimate = InnerFilter::new( + value, + estimate.time(), + self.config.step_threshold, + self.config.initial_frequency_uncertainty, + ); + } else { + estimate.absorb_delay_offset(value, variance); + } + } + if let Some(value) = measurement.peer_delay { + estimate.absorb_peer_delay(value.seconds(), variance); + } + + self.steer(clock) + } + + fn update(&mut self, clock: &mut C) -> FilterUpdate { + self.change_frequency(0.0, clock); + FilterUpdate { + next_update: None, + mean_delay: self.mean_delay(), + } + } + + fn demobilize(mut self, clock: &mut C) { + self.change_frequency(0.0, clock); + } + + fn current_estimates(&self) -> FilterEstimate { + FilterEstimate { + offset_from_master: Duration::from_seconds( + self.estimate.as_ref().map_or(0.0, InnerFilter::offset), + ), + mean_delay: self.mean_delay().unwrap_or(Duration::ZERO), + } + } +} + +impl FixedWanderKalmanFilter { + fn ensure_frequency(&mut self, clock: &mut C) { + if self.frequency.is_none() && clock.set_frequency(0.0).is_ok() { + self.frequency = Some(0.0); + } + } + + fn change_frequency(&mut self, target: f64, clock: &mut C) { + let (Some(current), Some(estimate)) = (self.frequency, self.estimate.as_mut()) else { + return; + }; + let requested = target - estimate.frequency() * 1e6; + let next = clamp( + current + requested, + -self.config.max_frequency, + self.config.max_frequency, + ); + let applied = next - current; + if let Ok(time) = clock.set_frequency(next) { + self.frequency = Some(next); + estimate.absorb_frequency_steer( + applied, + time, + self.config.frequency_wander, + self.config.delay_wander, + ); + } + } + + fn steer(&mut self, clock: &mut C) -> FilterUpdate { + let Some(estimate) = self.estimate.as_ref() else { + return FilterUpdate::default(); + }; + let offset = estimate.offset(); + if offset.abs() < self.config.step_threshold.seconds() { + let target = clamp( + -offset * 1e6 / self.config.steer_time.seconds(), + -self.config.max_steer, + self.config.max_steer, + ); + self.change_frequency(target, clock); + FilterUpdate { + next_update: Some(core::time::Duration::from_secs_f64( + self.config.steer_time.seconds(), + )), + mean_delay: self.mean_delay(), + } + } else { + if clock.step_clock(Duration::from_seconds(-offset)).is_ok() { + if let Some(estimate) = self.estimate.as_mut() { + estimate.absorb_offset_steer(-offset); + } + } + FilterUpdate { + next_update: None, + mean_delay: self.mean_delay(), + } + } + } + + fn mean_delay(&self) -> Option { + self.estimate + .as_ref() + .map(|estimate| Duration::from_seconds(estimate.delay())) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::config::TimePropertiesDS; + + #[derive(Debug)] + struct TestError; + + struct TestClock { + time: Time, + frequency: f64, + last_step: Option, + fail_frequency: bool, + } + + impl Clock for TestClock { + type Error = TestError; + + fn now(&self) -> Time { + self.time + } + + fn step_clock(&mut self, offset: Duration) -> Result { + self.last_step = Some(offset); + self.time += offset; + Ok(self.time) + } + + fn set_frequency(&mut self, ppm: f64) -> Result { + if self.fail_frequency { + return Err(TestError); + } + self.frequency = ppm; + Ok(self.time) + } + + fn set_properties(&mut self, _: &TimePropertiesDS) -> Result<(), Self::Error> { + Ok(()) + } + } + + fn test_clock(time: Time) -> TestClock { + TestClock { + time, + frequency: 0.0, + last_step: None, + fail_frequency: false, + } + } + + fn assert_close(actual: f64, expected: f64) { + assert!((actual - expected).abs() < 1e-12 * expected.abs().max(1.0)); + } + + #[test] + fn clamp_matches_float_contract() { + for (min, max) in [ + (-200.0, 200.0), + (0.0, 0.0), + (-0.0, 0.0), + (0.0, -0.0), + (f64::NEG_INFINITY, f64::INFINITY), + ] { + for value in [ + f64::NEG_INFINITY, + -400.0, + -200.0, + -0.0, + 0.0, + 200.0, + 400.0, + f64::INFINITY, + f64::from_bits(0x7ff8_0000_0000_0042), + ] { + assert_eq!( + clamp(value, min, max).to_bits(), + value.clamp(min, max).to_bits() + ); + } + } + } + + #[test] + #[should_panic(expected = "invalid clamp bounds")] + fn clamp_rejects_reversed_bounds() { + clamp(0.0, 1.0, -1.0); + } + + #[test] + #[should_panic(expected = "invalid clamp bounds")] + fn clamp_rejects_nan_lower_bound() { + clamp(0.0, f64::NAN, 1.0); + } + + #[test] + #[should_panic(expected = "invalid clamp bounds")] + fn clamp_rejects_nan_upper_bound() { + clamp(0.0, -1.0, f64::NAN); + } + + #[test] + fn measurement_noise_uses_startup_range_then_ring_variance() { + let config = FixedWanderKalmanConfig { + initial_measurement_uncertainty: Duration::from_seconds(0.5), + ..Default::default() + }; + let mut noise = MeasurementNoise::default(); + + for value in 0..3 { + noise.push(value as f64); + } + assert_close(noise.variance(&config), 0.25); + + noise.push(3.0); + assert_close(noise.variance(&config), 9.0); + + for value in 4..8 { + noise.push(value as f64); + } + assert_close(noise.variance(&config), 3.0); + + for value in 8..33 { + noise.push(value as f64); + } + // The ring now contains 1..=32. Their sample variance is 88, and + // Sync-minus-DelayReq variance is twice the one-way variance. + assert_close(noise.variance(&config), 44.0); + } + + #[test] + fn measurement_noise_pairs_sync_and_delay_with_frequency_correction() { + let mut noise = MeasurementNoise::default(); + noise.observe( + Measurement { + event_time: Time::from_nanos(1_000_000_000), + raw_sync_offset: Some(Duration::from_nanos(10)), + ..Measurement::default() + }, + 1e-6, + ); + noise.observe( + Measurement { + event_time: Time::from_nanos(1_100_000_000), + raw_delay_offset: Some(Duration::from_nanos(2)), + ..Measurement::default() + }, + 1e-6, + ); + + assert_eq!(noise.len, 1); + assert_close(noise.data[0], 108e-9); + } + + #[test] + fn positive_local_phase_error_commands_negative_frequency() { + let time = Time::from_nanos(1_000_000_000); + let mut clock = test_clock(time); + let mut filter = FixedWanderKalmanFilter::new(FixedWanderKalmanConfig::default()); + filter.measurement( + Measurement { + event_time: time, + raw_sync_offset: Some(Duration::from_nanos(100)), + ..Measurement::default() + }, + &mut clock, + ); + assert!(clock.frequency < 0.0); + } + + #[test] + fn frequency_command_respects_actuator_limit() { + let time = Time::from_nanos(1_000_000_000); + let mut clock = test_clock(time); + let mut filter = FixedWanderKalmanFilter::new(FixedWanderKalmanConfig { + max_frequency: 5.0, + ..Default::default() + }); + + filter.measurement( + Measurement { + event_time: time, + raw_sync_offset: Some(Duration::from_micros(100)), + ..Measurement::default() + }, + &mut clock, + ); + + assert_eq!(clock.frequency, -5.0); + } + + #[test] + fn large_phase_error_steps_the_clock() { + let time = Time::from_nanos(1_000_000_000); + let mut clock = test_clock(time); + let mut filter = FixedWanderKalmanFilter::new(FixedWanderKalmanConfig::default()); + + filter.measurement( + Measurement { + event_time: time, + raw_sync_offset: Some(Duration::from_millis(2)), + ..Measurement::default() + }, + &mut clock, + ); + + assert!( + (clock.last_step.unwrap() - Duration::from_millis(-2)).abs() < Duration::from_nanos(1) + ); + assert!(filter.current_estimates().offset_from_master.abs() < Duration::from_nanos(1)); + } + + #[test] + fn failed_frequency_initialization_is_not_assumed_applied() { + let time = Time::from_nanos(1_000_000_000); + let mut clock = test_clock(time); + clock.fail_frequency = true; + let mut filter = FixedWanderKalmanFilter::new(FixedWanderKalmanConfig::default()); + + filter.measurement( + Measurement { + event_time: time, + raw_sync_offset: Some(Duration::from_nanos(100)), + ..Measurement::default() + }, + &mut clock, + ); + + assert_eq!(filter.frequency, None); + assert_eq!(clock.frequency, 0.0); + } +} diff --git a/statime/src/filters/kalman.rs b/statime/src/filters/kalman.rs index 8f9a047e1..1e2fa34a0 100644 --- a/statime/src/filters/kalman.rs +++ b/statime/src/filters/kalman.rs @@ -1,7 +1,4 @@ -use super::{ - matrix::{Matrix, Vector}, - FilterEstimate, -}; +use super::FilterEstimate; #[allow(unused_imports)] use crate::float_polyfill::FloatPolyfill; use crate::{ @@ -236,131 +233,185 @@ impl MeasurementErrorEstimator { } } +/// State `x = [offset, residual fractional frequency, mean path delay]`. +#[derive(Clone, Copy, Debug)] +struct State { + offset: f64, + frequency: f64, + delay: f64, +} + +/// Independent entries of the symmetric covariance `P` for [`State`]. +#[derive(Clone, Copy, Debug)] +struct Covariance { + offset: f64, + offset_frequency: f64, + offset_delay: f64, + frequency: f64, + frequency_delay: f64, + delay: f64, +} + #[derive(Clone, Debug)] -struct InnerFilter { - state: Vector<3>, - uncertainty: Matrix<3, 3>, +pub(super) struct InnerFilter { + state: State, + uncertainty: Covariance, filter_time: Time, } impl InnerFilter { - const MEASUREMENT_SYNC: Matrix<1, 3> = Matrix::new([[1.0, 0.0, 1.0]]); - const MEASUREMENT_DELAY: Matrix<1, 3> = Matrix::new([[1.0, 0.0, -1.0]]); - const MEASUREMENT_PEER_DELAY: Matrix<1, 3> = Matrix::new([[0.0, 0.0, 1.0]]); - - fn new(initial_offset: f64, time: Time, config: &KalmanConfiguration) -> Self { + pub(super) fn new( + initial_offset: f64, + time: Time, + initial_offset_uncertainty: Duration, + initial_frequency_uncertainty: f64, + ) -> Self { Self { - state: Vector::new_vector([initial_offset, 0.0, 0.0]), - uncertainty: Matrix::new([ - [sqr(config.step_threshold.seconds()), 0.0, 0.0], - [0.0, sqr(config.initial_frequency_uncertainty), 0.0], - [0.0, 0.0, sqr(config.step_threshold.seconds())], - ]), + state: State { + offset: initial_offset, + frequency: 0.0, + delay: 0.0, + }, + uncertainty: Covariance { + offset: sqr(initial_offset_uncertainty.seconds()), + offset_frequency: 0.0, + offset_delay: 0.0, + frequency: sqr(initial_frequency_uncertainty), + frequency_delay: 0.0, + delay: sqr(initial_offset_uncertainty.seconds()), + }, filter_time: time, } } - fn progress_filtertime(&mut self, time: Time, wander: f64, config: &KalmanConfiguration) { + pub(super) fn progress_filtertime(&mut self, time: Time, wander: f64, delay_wander: f64) { debug_assert!(time >= self.filter_time); if time < self.filter_time { return; } let delta_t = (time - self.filter_time).seconds(); - let update = Matrix::new([[1.0, delta_t, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]); - let process_noise = Matrix::new([ - [ - wander * delta_t * delta_t * delta_t / 3., - wander * delta_t * delta_t / 2., - 0., - ], - [wander * delta_t * delta_t / 2., wander * delta_t, 0.], - [ - 0., - 0., - config.delay_wander * delta_t * sqr(self.state.ventry(2)), - ], - ]); - - self.state = update * self.state; - self.uncertainty = update * self.uncertainty * update.transpose() + process_noise; + let delta_t2 = delta_t * delta_t; + + // Expand F P F^T + Q for F=[[1,dt,0],[0,1,0],[0,0,1]]. + self.state.offset += delta_t * self.state.frequency; + self.uncertainty.offset += 2.0 * delta_t * self.uncertainty.offset_frequency + + delta_t2 * self.uncertainty.frequency + + wander * delta_t2 * delta_t / 3.0; + self.uncertainty.offset_frequency += + delta_t * self.uncertainty.frequency + wander * delta_t2 / 2.0; + self.uncertainty.offset_delay += delta_t * self.uncertainty.frequency_delay; + self.uncertainty.frequency += wander * delta_t; + self.uncertainty.delay += delay_wander * delta_t * sqr(self.state.delay); self.filter_time = time; } - fn absorb_sync_offset(&mut self, sync_offset: f64, variance: f64) { - let measurement_vec = Vector::new_vector([sync_offset]); - let measurement_noise = Matrix::new([[variance]]); - self.absorb_measurement(measurement_vec, Self::MEASUREMENT_SYNC, measurement_noise); + pub(super) fn absorb_sync_offset(&mut self, sync_offset: f64, variance: f64) { + // h = [1, 0, 1]. + let projected = [ + self.uncertainty.offset + self.uncertainty.offset_delay, + self.uncertainty.offset_frequency + self.uncertainty.frequency_delay, + self.uncertainty.offset_delay + self.uncertainty.delay, + ]; + self.absorb_measurement( + sync_offset - self.state.offset - self.state.delay, + variance + projected[0] + projected[2], + projected, + ); } - fn absorb_delay_offset(&mut self, delay_offset: f64, variance: f64) { - let measurement_vec = Vector::new_vector([delay_offset]); - let measurement_noise = Matrix::new([[variance]]); - self.absorb_measurement(measurement_vec, Self::MEASUREMENT_DELAY, measurement_noise); + pub(super) fn absorb_delay_offset(&mut self, delay_offset: f64, variance: f64) { + // h = [1, 0, -1]. + let projected = [ + self.uncertainty.offset - self.uncertainty.offset_delay, + self.uncertainty.offset_frequency - self.uncertainty.frequency_delay, + self.uncertainty.offset_delay - self.uncertainty.delay, + ]; + self.absorb_measurement( + delay_offset - self.state.offset + self.state.delay, + variance + projected[0] - projected[2], + projected, + ); } - fn absorb_peer_delay(&mut self, peer_delay: f64, variance: f64) { - let measurement_vec = Vector::new_vector([peer_delay]); - let measurement_noise = Matrix::new([[variance]]); + pub(super) fn absorb_peer_delay(&mut self, peer_delay: f64, variance: f64) { + // h = [0, 0, 1]. + let projected = [ + self.uncertainty.offset_delay, + self.uncertainty.frequency_delay, + self.uncertainty.delay, + ]; self.absorb_measurement( - measurement_vec, - Self::MEASUREMENT_PEER_DELAY, - measurement_noise, - ) + peer_delay - self.state.delay, + variance + projected[2], + projected, + ); } fn absorb_measurement( &mut self, - measurement_vec: Vector<1>, - measurement_transform: Matrix<1, 3>, - measurement_noise: Matrix<1, 1>, + innovation: f64, + innovation_variance: f64, + projected: [f64; 3], ) { - let (prediction, uncertainty) = self.predict(measurement_transform); - - let difference = measurement_vec - prediction; - let difference_covariance = uncertainty + measurement_noise; - let update_strength = - self.uncertainty * measurement_transform.transpose() * difference_covariance.inverse(); - self.state = self.state + update_strength * difference; - self.uncertainty = ((Matrix::unit() - update_strength * measurement_transform) - * self.uncertainty) - .symmetrize(); + // With u=P h^T and S=h u+R: x+=u*innovation/S and P-=u u^T/S. + let scale = 1.0 / innovation_variance; + self.state.offset += projected[0] * innovation * scale; + self.state.frequency += projected[1] * innovation * scale; + self.state.delay += projected[2] * innovation * scale; + + self.uncertainty.offset -= projected[0] * projected[0] * scale; + self.uncertainty.offset_frequency -= projected[0] * projected[1] * scale; + self.uncertainty.offset_delay -= projected[0] * projected[2] * scale; + self.uncertainty.frequency -= projected[1] * projected[1] * scale; + self.uncertainty.frequency_delay -= projected[1] * projected[2] * scale; + self.uncertainty.delay -= projected[2] * projected[2] * scale; } - fn absorb_frequency_steer( + pub(super) fn absorb_frequency_steer( &mut self, steer: f64, time: Time, wander: f64, - config: &KalmanConfiguration, + delay_wander: f64, ) { - self.progress_filtertime(time, wander, config); - self.state = self.state + Vector::new_vector([0., steer * 1e-6, 0.]); + self.progress_filtertime(time, wander, delay_wander); + self.state.frequency += steer * 1e-6; } - fn absorb_offset_steer(&mut self, steer: f64) { - self.state = self.state + Vector::new_vector([steer, 0., 0.]); + pub(super) fn absorb_offset_steer(&mut self, steer: f64) { + self.state.offset += steer; self.filter_time += Duration::from_seconds(steer); } - fn predict( - &self, - measurement_transform: Matrix, - ) -> (Vector, Matrix) { - let prediction = measurement_transform * self.state; - let uncertainty = - measurement_transform * self.uncertainty * measurement_transform.transpose(); - (prediction, uncertainty) - } - fn predict_sync_offset(&self) -> (f64, f64) { - let (prediction, uncertainty) = self.predict(Self::MEASUREMENT_SYNC); - (prediction.entry(0, 0), uncertainty.entry(0, 0)) + ( + self.state.offset + self.state.delay, + self.uncertainty.offset + 2.0 * self.uncertainty.offset_delay + self.uncertainty.delay, + ) } fn predict_delay_offset(&self) -> (f64, f64) { - let (prediction, uncertainty) = self.predict(Self::MEASUREMENT_DELAY); - (prediction.entry(0, 0), uncertainty.entry(0, 0)) + ( + self.state.offset - self.state.delay, + self.uncertainty.offset - 2.0 * self.uncertainty.offset_delay + self.uncertainty.delay, + ) + } + + pub(super) fn offset(&self) -> f64 { + self.state.offset + } + + pub(super) fn frequency(&self) -> f64 { + self.state.frequency + } + + pub(super) fn delay(&self) -> f64 { + self.state.delay + } + + pub(super) fn time(&self) -> Time { + self.filter_time } } @@ -374,8 +425,15 @@ impl BaseFilter { fn progress_filtertime(&mut self, time: Time, wander: f64, config: &KalmanConfiguration) { match &mut self.0 { - Some(inner) => inner.progress_filtertime(time, wander, config), - None => self.0 = Some(InnerFilter::new(0.0, time, config)), + Some(inner) => inner.progress_filtertime(time, wander, config.delay_wander), + None => { + self.0 = Some(InnerFilter::new( + 0.0, + time, + config.step_threshold, + config.initial_frequency_uncertainty, + )) + } } } @@ -386,9 +444,14 @@ impl BaseFilter { config: &KalmanConfiguration, ) { if let Some(inner) = &mut self.0 { - if (sync_offset - inner.state.ventry(0)).abs() > config.step_threshold.seconds() { + if (sync_offset - inner.state.offset).abs() > config.step_threshold.seconds() { log::info!("Measurement too far from state, resetting"); - *inner = InnerFilter::new(sync_offset, inner.filter_time, config); + *inner = InnerFilter::new( + sync_offset, + inner.filter_time, + config.step_threshold, + config.initial_frequency_uncertainty, + ); } else { inner.absorb_sync_offset(sync_offset, variance) } @@ -402,9 +465,14 @@ impl BaseFilter { config: &KalmanConfiguration, ) { if let Some(inner) = &mut self.0 { - if (delay_offset - inner.state.ventry(0)).abs() > config.step_threshold.seconds() { + if (delay_offset - inner.state.offset).abs() > config.step_threshold.seconds() { log::info!("Measurement too far from state, resetting"); - *inner = InnerFilter::new(delay_offset, inner.filter_time, config); + *inner = InnerFilter::new( + delay_offset, + inner.filter_time, + config.step_threshold, + config.initial_frequency_uncertainty, + ); } else { inner.absorb_delay_offset(delay_offset, variance) } @@ -425,8 +493,15 @@ impl BaseFilter { config: &KalmanConfiguration, ) { match &mut self.0 { - Some(inner) => inner.absorb_frequency_steer(steer, time, wander, config), - None => self.0 = Some(InnerFilter::new(0.0, time, config)), + Some(inner) => inner.absorb_frequency_steer(steer, time, wander, config.delay_wander), + None => { + self.0 = Some(InnerFilter::new( + 0.0, + time, + config.step_threshold, + config.initial_frequency_uncertainty, + )) + } } } @@ -439,42 +514,42 @@ impl BaseFilter { fn offset(&self) -> f64 { self.0 .as_ref() - .map(|inner| inner.state.ventry(0)) + .map(|inner| inner.state.offset) .unwrap_or(0.0) } fn offset_uncertainty(&self, config: &KalmanConfiguration) -> f64 { self.0 .as_ref() - .map(|inner| inner.uncertainty.entry(0, 0).sqrt()) + .map(|inner| inner.uncertainty.offset.sqrt()) .unwrap_or(config.step_threshold.seconds()) } fn freq_offset(&self) -> f64 { self.0 .as_ref() - .map(|inner| inner.state.ventry(1)) + .map(|inner| inner.state.frequency) .unwrap_or(0.0) } fn freq_offset_uncertainty(&self, config: &KalmanConfiguration) -> f64 { self.0 .as_ref() - .map(|inner| inner.uncertainty.entry(1, 1).sqrt()) + .map(|inner| inner.uncertainty.frequency.sqrt()) .unwrap_or(config.initial_frequency_uncertainty) } fn mean_delay(&self) -> f64 { self.0 .as_ref() - .map(|inner| inner.state.ventry(2)) + .map(|inner| inner.state.delay) .unwrap_or(0.0) } fn mean_delay_uncertainty(&self, config: &KalmanConfiguration) -> f64 { self.0 .as_ref() - .map(|inner| inner.uncertainty.entry(2, 2).sqrt()) + .map(|inner| inner.uncertainty.delay.sqrt()) .unwrap_or(config.step_threshold.seconds()) } @@ -790,11 +865,329 @@ impl KalmanFilter { } } +#[cfg(test)] +#[rustfmt::skip] +mod dense_reference { + use super::*; + use crate::filters::matrix::{Matrix, Vector}; + + // This is the previous matrix implementation, retained verbatim as the + // executable reference for the specialized scalar algebra. + #[derive(Clone, Debug)] + struct InnerFilter { + state: Vector<3>, + uncertainty: Matrix<3, 3>, + filter_time: Time, + } + + impl InnerFilter { + const MEASUREMENT_SYNC: Matrix<1, 3> = Matrix::new([[1.0, 0.0, 1.0]]); + const MEASUREMENT_DELAY: Matrix<1, 3> = Matrix::new([[1.0, 0.0, -1.0]]); + const MEASUREMENT_PEER_DELAY: Matrix<1, 3> = Matrix::new([[0.0, 0.0, 1.0]]); + + fn new(initial_offset: f64, time: Time, config: &KalmanConfiguration) -> Self { + Self { + state: Vector::new_vector([initial_offset, 0.0, 0.0]), + uncertainty: Matrix::new([ + [sqr(config.step_threshold.seconds()), 0.0, 0.0], + [0.0, sqr(config.initial_frequency_uncertainty), 0.0], + [0.0, 0.0, sqr(config.step_threshold.seconds())], + ]), + filter_time: time, + } + } + + fn progress_filtertime(&mut self, time: Time, wander: f64, config: &KalmanConfiguration) { + debug_assert!(time >= self.filter_time); + if time < self.filter_time { + return; + } + + let delta_t = (time - self.filter_time).seconds(); + let update = Matrix::new([[1.0, delta_t, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]); + let process_noise = Matrix::new([ + [ + wander * delta_t * delta_t * delta_t / 3., + wander * delta_t * delta_t / 2., + 0., + ], + [wander * delta_t * delta_t / 2., wander * delta_t, 0.], + [ + 0., + 0., + config.delay_wander * delta_t * sqr(self.state.ventry(2)), + ], + ]); + + self.state = update * self.state; + self.uncertainty = update * self.uncertainty * update.transpose() + process_noise; + self.filter_time = time; + } + + fn absorb_sync_offset(&mut self, sync_offset: f64, variance: f64) { + let measurement_vec = Vector::new_vector([sync_offset]); + let measurement_noise = Matrix::new([[variance]]); + self.absorb_measurement(measurement_vec, Self::MEASUREMENT_SYNC, measurement_noise); + } + + fn absorb_delay_offset(&mut self, delay_offset: f64, variance: f64) { + let measurement_vec = Vector::new_vector([delay_offset]); + let measurement_noise = Matrix::new([[variance]]); + self.absorb_measurement(measurement_vec, Self::MEASUREMENT_DELAY, measurement_noise); + } + + fn absorb_peer_delay(&mut self, peer_delay: f64, variance: f64) { + let measurement_vec = Vector::new_vector([peer_delay]); + let measurement_noise = Matrix::new([[variance]]); + self.absorb_measurement( + measurement_vec, + Self::MEASUREMENT_PEER_DELAY, + measurement_noise, + ) + } + + fn absorb_measurement( + &mut self, + measurement_vec: Vector<1>, + measurement_transform: Matrix<1, 3>, + measurement_noise: Matrix<1, 1>, + ) { + let (prediction, uncertainty) = self.predict(measurement_transform); + + let difference = measurement_vec - prediction; + let difference_covariance = uncertainty + measurement_noise; + let update_strength = + self.uncertainty * measurement_transform.transpose() * difference_covariance.inverse(); + self.state = self.state + update_strength * difference; + self.uncertainty = ((Matrix::unit() - update_strength * measurement_transform) + * self.uncertainty) + .symmetrize(); + } + + fn absorb_frequency_steer( + &mut self, + steer: f64, + time: Time, + wander: f64, + config: &KalmanConfiguration, + ) { + self.progress_filtertime(time, wander, config); + self.state = self.state + Vector::new_vector([0., steer * 1e-6, 0.]); + } + + fn absorb_offset_steer(&mut self, steer: f64) { + self.state = self.state + Vector::new_vector([steer, 0., 0.]); + self.filter_time += Duration::from_seconds(steer); + } + + fn predict( + &self, + measurement_transform: Matrix, + ) -> (Vector, Matrix) { + let prediction = measurement_transform * self.state; + let uncertainty = + measurement_transform * self.uncertainty * measurement_transform.transpose(); + (prediction, uncertainty) + } + + fn predict_sync_offset(&self) -> (f64, f64) { + let (prediction, uncertainty) = self.predict(Self::MEASUREMENT_SYNC); + (prediction.entry(0, 0), uncertainty.entry(0, 0)) + } + + fn predict_delay_offset(&self) -> (f64, f64) { + let (prediction, uncertainty) = self.predict(Self::MEASUREMENT_DELAY); + (prediction.entry(0, 0), uncertainty.entry(0, 0)) + } + } + + struct TestRng(u64); + + impl TestRng { + fn next(&mut self) -> u64 { + self.0 ^= self.0 << 13; + self.0 ^= self.0 >> 7; + self.0 ^= self.0 << 17; + self.0 + } + + fn unit(&mut self) -> f64 { + (self.next() >> 11) as f64 / (1_u64 << 53) as f64 + } + + fn signed(&mut self) -> f64 { + 2.0 * self.unit() - 1.0 + } + } + + fn assert_close(actual: f64, expected: f64) { + let scale = actual.abs().max(expected.abs()).max(1.0); + assert!( + (actual - expected).abs() <= 1e-12 * scale, + "actual {actual:e}, expected {expected:e}" + ); + } + + fn assert_equivalent(actual: &super::InnerFilter, expected: &InnerFilter) { + let actual_state = [ + actual.state.offset, + actual.state.frequency, + actual.state.delay, + ]; + let actual_covariance = [ + [ + actual.uncertainty.offset, + actual.uncertainty.offset_frequency, + actual.uncertainty.offset_delay, + ], + [ + actual.uncertainty.offset_frequency, + actual.uncertainty.frequency, + actual.uncertainty.frequency_delay, + ], + [ + actual.uncertainty.offset_delay, + actual.uncertainty.frequency_delay, + actual.uncertainty.delay, + ], + ]; + + for (i, (actual_state, actual_covariance)) in + actual_state.iter().zip(&actual_covariance).enumerate() + { + assert_close(*actual_state, expected.state.ventry(i)); + for (j, actual_covariance) in actual_covariance.iter().enumerate() { + assert_close(*actual_covariance, expected.uncertainty.entry(i, j)); + } + } + assert_eq!(actual.filter_time, expected.filter_time); + } + + #[test] + fn scalar_algebra_matches_previous_matrix_implementation() { + let mut rng = TestRng(0x4d59_5df4_d0f3_3173); + + for _ in 0..128 { + let state = [rng.signed(), rng.signed() * 1e-3, rng.signed()]; + let factor: [[f64; 3]; 3] = + core::array::from_fn(|_| core::array::from_fn(|_| rng.signed())); + let covariance = core::array::from_fn::<_, 3, _>(|i| { + core::array::from_fn::<_, 3, _>(|j| { + (0..3).map(|k| factor[i][k] * factor[j][k]).sum::() + + if i == j { 0.1 } else { 0.0 } + }) + }); + let timebase = Time::from_nanos(0); + let mut actual = super::InnerFilter { + state: State { + offset: state[0], + frequency: state[1], + delay: state[2], + }, + uncertainty: Covariance { + offset: covariance[0][0], + offset_frequency: covariance[0][1], + offset_delay: covariance[0][2], + frequency: covariance[1][1], + frequency_delay: covariance[1][2], + delay: covariance[2][2], + }, + filter_time: timebase, + }; + let mut expected = InnerFilter { + state: Vector::new_vector(state), + uncertainty: Matrix::new(covariance), + filter_time: timebase, + }; + let config = KalmanConfiguration { + delay_wander: rng.unit() * 1e-3, + ..Default::default() + }; + let wander = rng.unit() * 1e-6; + let time = Time::from_nanos(1 + rng.next() % 10_000_000_000); + + actual.progress_filtertime(time, wander, config.delay_wander); + expected.progress_filtertime(time, wander, &config); + assert_equivalent(&actual, &expected); + + for (actual_prediction, expected_prediction) in [ + (actual.predict_sync_offset(), expected.predict_sync_offset()), + ( + actual.predict_delay_offset(), + expected.predict_delay_offset(), + ), + ] { + assert_close(actual_prediction.0, expected_prediction.0); + assert_close(actual_prediction.1, expected_prediction.1); + } + + let value = rng.signed(); + let variance = 0.1 + rng.unit(); + actual.absorb_sync_offset(value, variance); + expected.absorb_sync_offset(value, variance); + assert_equivalent(&actual, &expected); + + let value = rng.signed(); + let variance = 0.1 + rng.unit(); + actual.absorb_delay_offset(value, variance); + expected.absorb_delay_offset(value, variance); + assert_equivalent(&actual, &expected); + + let value = rng.signed(); + let variance = 0.1 + rng.unit(); + actual.absorb_peer_delay(value, variance); + expected.absorb_peer_delay(value, variance); + assert_equivalent(&actual, &expected); + + let next_time = time + Duration::from_fixed_nanos(rng.unit() * 1e9); + let steer = rng.signed() * 100.0; + actual.absorb_frequency_steer(steer, next_time, wander, config.delay_wander); + expected.absorb_frequency_steer(steer, next_time, wander, &config); + assert_equivalent(&actual, &expected); + + let steer = rng.signed() * 1e-3; + actual.absorb_offset_steer(steer); + expected.absorb_offset_steer(steer); + assert_equivalent(&actual, &expected); + } + + // Keep the constructor in the reference coverage as well. + let config = KalmanConfiguration::default(); + let actual = super::InnerFilter::new( + 0.25, + Time::from_nanos(42), + config.step_threshold, + config.initial_frequency_uncertainty, + ); + let expected = InnerFilter::new(0.25, Time::from_nanos(42), &config); + assert_equivalent(&actual, &expected); + } +} + #[cfg(test)] mod tests { use super::*; use crate::Clock; + fn inner_filter(time: Time) -> InnerFilter { + InnerFilter { + state: State { + offset: 0.0, + frequency: 0.0, + delay: 0.0, + }, + uncertainty: Covariance { + offset: 1e-17, + offset_frequency: 0.0, + offset_delay: 0.0, + frequency: 1e-16, + frequency_delay: 0.0, + delay: 1e-18, + }, + filter_time: time, + } + } + #[derive(Default)] struct TestClock { last_freq: Option, @@ -832,11 +1225,7 @@ mod tests { max_freq_offset: 10.0, ..Default::default() }, - running_filter: BaseFilter(Some(InnerFilter { - state: Vector::new_vector([0.0, 0.0, 0.0]), - uncertainty: Matrix::new([[1e-17, 0.0, 0.0], [0.0, 1e-16, 0.0], [0.0, 0.0, 1e-18]]), - filter_time: timebase, - })), + running_filter: BaseFilter(Some(inner_filter(timebase))), wander_filter: BaseFilter(None), wander_score: 0, wander: KalmanConfiguration::default().initial_wander, @@ -858,11 +1247,7 @@ mod tests { max_freq_offset: 10.0, ..Default::default() }, - running_filter: BaseFilter(Some(InnerFilter { - state: Vector::new_vector([0.0, 0.0, 0.0]), - uncertainty: Matrix::new([[1e-17, 0.0, 0.0], [0.0, 1e-16, 0.0], [0.0, 0.0, 1e-18]]), - filter_time: timebase, - })), + running_filter: BaseFilter(Some(inner_filter(timebase))), wander_filter: BaseFilter(None), wander_score: 0, wander: KalmanConfiguration::default().initial_wander, @@ -884,11 +1269,7 @@ mod tests { max_freq_offset: 10.0, ..Default::default() }, - running_filter: BaseFilter(Some(InnerFilter { - state: Vector::new_vector([0.0, 0.0, 0.0]), - uncertainty: Matrix::new([[1e-17, 0.0, 0.0], [0.0, 1e-16, 0.0], [0.0, 0.0, 1e-18]]), - filter_time: timebase, - })), + running_filter: BaseFilter(Some(inner_filter(timebase))), wander_filter: BaseFilter(None), wander_score: 0, wander: KalmanConfiguration::default().initial_wander, @@ -906,11 +1287,7 @@ mod tests { max_freq_offset: 10.0, ..Default::default() }, - running_filter: BaseFilter(Some(InnerFilter { - state: Vector::new_vector([0.0, 0.0, 0.0]), - uncertainty: Matrix::new([[1e-17, 0.0, 0.0], [0.0, 1e-16, 0.0], [0.0, 0.0, 1e-18]]), - filter_time: timebase, - })), + running_filter: BaseFilter(Some(inner_filter(timebase))), wander_filter: BaseFilter(None), wander_score: 0, wander: KalmanConfiguration::default().initial_wander, diff --git a/statime/src/filters/mod.rs b/statime/src/filters/mod.rs index 7102ba4f5..32117758f 100644 --- a/statime/src/filters/mod.rs +++ b/statime/src/filters/mod.rs @@ -1,10 +1,13 @@ //! Definitions and implementations for the abstracted measurement filters mod basic; +mod fixed_wander; mod kalman; +#[cfg(test)] mod matrix; pub use basic::BasicFilter; +pub use fixed_wander::{FixedWanderKalmanConfig, FixedWanderKalmanFilter}; pub use kalman::{KalmanConfiguration, KalmanFilter}; use crate::{port::Measurement, time::Duration, Clock};