Skip to content

Commit

Permalink
Reorganized the main calculation code in the kalman filter.
Browse files Browse the repository at this point in the history
  • Loading branch information
@davidv1992 @rnijveld
davidv1992 authored and rnijveld committed May 16, 2024
1 parent d7d2aeb commit 9e031b9
Show file tree
Hide file tree
Showing 4 changed files with 322 additions and 205 deletions.
86 changes: 41 additions & 45 deletions ntp-proto/src/algorithm/kalman/combiner.rs
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,9 @@
use crate::{packet::NtpLeapIndicator, time_types::NtpDuration};

use super::{
config::AlgorithmConfig,
matrix::{Matrix, Vector},
sqr, SourceSnapshot,
};
use super::{config::AlgorithmConfig, source::KalmanState, SourceSnapshot};

pub(super) struct Combine<Index: Copy> {
pub estimate: Vector<2>,
pub uncertainty: Matrix<2, 2>,
pub estimate: KalmanState,
pub sources: Vec<Index>,
pub delay: NtpDuration,
pub leap_indicator: Option<NtpLeapIndicator>,
Expand Down Expand Up @@ -44,41 +39,31 @@ pub(super) fn combine<Index: Copy>(
algo_config: &AlgorithmConfig,
) -> Option<Combine<Index>> {
selection.first().map(|first| {
let mut estimate = first.state;
let mut uncertainty = if algo_config.ignore_server_dispersion {
first.uncertainty
} else {
first.uncertainty
+ Matrix::new([[sqr(first.source_uncertainty.to_seconds()), 0.], [0., 0.]])
};
let mut estimate = first.state.clone();
if !algo_config.ignore_server_dispersion {
estimate = estimate.add_server_dispersion(first.source_uncertainty.to_seconds())
}

let mut used_sources = vec![(first.index, uncertainty.determinant())];
let mut used_sources = vec![(first.index, estimate.uncertainty.determinant())];

for snapshot in selection.iter().skip(1) {
let source_estimate = snapshot.state;
let source_uncertainty = if algo_config.ignore_server_dispersion {
snapshot.uncertainty
let source_estimate = if algo_config.ignore_server_dispersion {
snapshot.state.clone()
} else {
snapshot.uncertainty
+ Matrix::new([
[sqr(snapshot.source_uncertainty.to_seconds()), 0.],
[0., 0.],
])
snapshot
.state
.add_server_dispersion(snapshot.source_uncertainty.to_seconds())
};

used_sources.push((snapshot.index, source_uncertainty.determinant()));
used_sources.push((snapshot.index, source_estimate.uncertainty.determinant()));

// Merge measurements
let mixer = (uncertainty + source_uncertainty).inverse();
estimate = estimate + uncertainty * mixer * (source_estimate - estimate);
uncertainty = uncertainty * mixer * source_uncertainty;
estimate = estimate.merge(&source_estimate);
}

used_sources.sort_by(|a, b| a.1.total_cmp(&b.1));

Combine {
estimate,
uncertainty,
sources: used_sources.iter().map(|v| v.0).collect(),
delay: selection
.iter()
Expand All @@ -92,7 +77,13 @@ pub(super) fn combine<Index: Copy>(

#[cfg(test)]
mod tests {
use crate::time_types::NtpTimestamp;
use crate::{
algorithm::kalman::{
matrix::{Matrix, Vector},
source::KalmanState,
},
time_types::NtpTimestamp,
};

use super::*;

Expand All @@ -103,8 +94,11 @@ mod tests {
) -> SourceSnapshot<usize> {
SourceSnapshot {
index: 0,
state,
uncertainty,
state: KalmanState {
state,
uncertainty,
time: NtpTimestamp::from_fixed_int(0),
},
delay: 0.0,
source_uncertainty: NtpDuration::from_seconds(source_uncertainty),
source_delay: NtpDuration::from_seconds(0.01),
Expand Down Expand Up @@ -132,15 +126,14 @@ mod tests {
..Default::default()
};
let result = combine(&selected, &algconfig).unwrap();
assert!((result.uncertainty.entry(0, 0) - 2e-6).abs() < 1e-12);
assert!((result.uncertainty.entry(0, 0) - 2e-6).abs() < 1e-12);
assert!((result.estimate.offset_variance() - 2e-6).abs() < 1e-12);

let algconfig = AlgorithmConfig {
ignore_server_dispersion: true,
..Default::default()
};
let result = combine(&selected, &algconfig).unwrap();
assert!((result.uncertainty.entry(0, 0) - 1e-6).abs() < 1e-12);
assert!((result.estimate.offset_variance() - 1e-6).abs() < 1e-12);
}

#[test]
Expand All @@ -162,20 +155,20 @@ mod tests {
..Default::default()
};
let result = combine(&selected, &algconfig).unwrap();
assert!((result.estimate.ventry(0) - 5e-4).abs() < 1e-8);
assert!(result.estimate.ventry(1).abs() < 1e-8);
assert!((result.uncertainty.entry(0, 0) - 1e-6).abs() < 1e-12);
assert!((result.uncertainty.entry(1, 1) - 5e-13).abs() < 1e-16);
assert!((result.estimate.offset() - 5e-4).abs() < 1e-8);
assert!(result.estimate.frequency().abs() < 1e-8);
assert!((result.estimate.offset_variance() - 1e-6).abs() < 1e-12);
assert!((result.estimate.frequency_variance() - 5e-13).abs() < 1e-16);

let algconfig = AlgorithmConfig {
ignore_server_dispersion: true,
..Default::default()
};
let result = combine(&selected, &algconfig).unwrap();
assert!((result.estimate.ventry(0) - 5e-4).abs() < 1e-8);
assert!(result.estimate.ventry(1).abs() < 1e-8);
assert!((result.uncertainty.entry(0, 0) - 5e-7).abs() < 1e-12);
assert!((result.uncertainty.entry(1, 1) - 5e-13).abs() < 1e-16);
assert!((result.estimate.offset() - 5e-4).abs() < 1e-8);
assert!(result.estimate.frequency().abs() < 1e-8);
assert!((result.estimate.offset_variance() - 5e-7).abs() < 1e-12);
assert!((result.estimate.frequency_variance() - 5e-13).abs() < 1e-16);
}

#[test]
Expand Down Expand Up @@ -226,8 +219,11 @@ mod tests {
fn snapshot_for_leap(leap: NtpLeapIndicator) -> SourceSnapshot<usize> {
SourceSnapshot {
index: 0,
state: Vector::new_vector([0.0, 0.0]),
uncertainty: Matrix::new([[1e-6, 0.0], [0.0, 1e-12]]),
state: KalmanState {
state: Vector::new_vector([0.0, 0.0]),
uncertainty: Matrix::new([[1e-6, 0.0], [0.0, 1e-12]]),
time: NtpTimestamp::from_fixed_int(0),
},
delay: 0.0,
source_uncertainty: NtpDuration::from_seconds(0.0),
source_delay: NtpDuration::from_seconds(0.0),
Expand Down
28 changes: 13 additions & 15 deletions ntp-proto/src/algorithm/kalman/mod.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,7 @@ use crate::{
use self::{
combiner::combine,
config::AlgorithmConfig,
matrix::{Matrix, Vector},
source::SourceState,
source::{KalmanState, SourceState},
};

use super::{ObservableSourceTimedata, StateUpdate, TimeSyncController};
Expand All @@ -33,8 +32,7 @@ fn sqr(x: f64) -> f64 {
#[derive(Debug, Clone)]
struct SourceSnapshot<Index: Copy> {
index: Index,
state: Vector<2>,
uncertainty: Matrix<2, 2>,
state: KalmanState,
delay: f64,

source_uncertainty: NtpDuration,
Expand All @@ -46,11 +44,11 @@ struct SourceSnapshot<Index: Copy> {

impl<Index: Copy> SourceSnapshot<Index> {
fn offset(&self) -> f64 {
self.state.ventry(0)
self.state.offset()
}

fn offset_uncertainty(&self) -> f64 {
self.uncertainty.entry(0, 0).sqrt()
self.state.offset_variance().sqrt()
}

fn observe(&self) -> ObservableSourceTimedata {
Expand Down Expand Up @@ -126,16 +124,16 @@ impl<C: NtpClock, SourceId: Hash + Eq + Copy + Debug> KalmanClockController<C, S
if let Some(combined) = combine(&selection, &self.algo_config) {
info!(
"Offset: {}+-{}ms, frequency: {}+-{}ppm",
combined.estimate.ventry(0) * 1e3,
combined.uncertainty.entry(0, 0).sqrt() * 1e3,
combined.estimate.ventry(1) * 1e6,
combined.uncertainty.entry(1, 1).sqrt() * 1e6
combined.estimate.offset() * 1e3,
combined.estimate.offset_variance().sqrt() * 1e3,
combined.estimate.frequency() * 1e6,
combined.estimate.frequency_variance().sqrt() * 1e6
);

let freq_delta = combined.estimate.ventry(1) - self.desired_freq;
let freq_uncertainty = combined.uncertainty.entry(1, 1).sqrt();
let offset_delta = combined.estimate.ventry(0);
let offset_uncertainty = combined.uncertainty.entry(0, 0).sqrt();
let freq_delta = combined.estimate.frequency() - self.desired_freq;
let freq_uncertainty = combined.estimate.frequency_variance().sqrt();
let offset_delta = combined.estimate.offset();
let offset_uncertainty = combined.estimate.offset_variance().sqrt();
let next_update = if self.desired_freq == 0.0
&& offset_delta.abs() > offset_uncertainty * self.algo_config.steer_offset_threshold
{
Expand Down Expand Up @@ -170,7 +168,7 @@ impl<C: NtpClock, SourceId: Hash + Eq + Copy + Debug> KalmanClockController<C, S

self.timedata.root_delay = combined.delay;
self.timedata.root_dispersion =
NtpDuration::from_seconds(combined.uncertainty.entry(0, 0).sqrt());
NtpDuration::from_seconds(combined.estimate.offset_variance().sqrt());
self.clock
.error_estimate_update(self.timedata.root_dispersion, self.timedata.root_delay)
.expect("Cannot update clock");
Expand Down
8 changes: 6 additions & 2 deletions ntp-proto/src/algorithm/kalman/select.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,7 @@ pub(super) fn select<Index: Copy>(
#[cfg(test)]
mod tests {
use crate::{
algorithm::kalman::source::KalmanState,
packet::NtpLeapIndicator,
time_types::{NtpDuration, NtpTimestamp},
};
Expand All @@ -87,8 +88,11 @@ mod tests {
fn snapshot_for_range(center: f64, uncertainty: f64, delay: f64) -> SourceSnapshot<usize> {
SourceSnapshot {
index: 0,
state: Vector::new_vector([center, 0.0]),
uncertainty: Matrix::new([[sqr(uncertainty), 0.0], [0.0, 10e-12]]),
state: KalmanState {
state: Vector::new_vector([center, 0.0]),
uncertainty: Matrix::new([[sqr(uncertainty), 0.0], [0.0, 10e-12]]),
time: NtpTimestamp::from_fixed_int(0),
},
delay,
source_uncertainty: NtpDuration::from_seconds(0.01),
source_delay: NtpDuration::from_seconds(0.01),
Expand Down

0 comments on commit 9e031b9

Please sign in to comment.