use std::collections::VecDeque;
use super::{add, identity, mul, mul_vec, outer, transpose, Mat, Predictor};
type Vec2 = [f64; 2];
type Vec4 = [f64; 4];
type Mat4 = Mat<4>;
const CM_PER_PX: f64 = 2.54 / 96.0;
const MAX_TIME_DELTA: f64 = 20.0;
#[derive(Clone, Debug)]
pub struct Params {
pub process_noise: f64,
pub measurement_noise: f64,
pub min_stable_iteration: u32,
pub max_time_samples: usize,
pub acceleration_weight: f64,
pub jerk_weight: f64,
pub prediction_interval: f64,
pub min_output_rate: f64,
pub desired_number_of_samples: u32,
pub max_estimation_distance: f64,
pub min_travel_speed: f64,
pub max_travel_speed: f64,
pub max_linear_deviation: f64,
pub baseline_linearity_confidence: f64,
}
pub const DEFAULT: Params = Params {
process_noise: 0.00026458,
measurement_noise: 0.026458,
min_stable_iteration: 4,
max_time_samples: 20,
acceleration_weight: 0.5,
jerk_weight: 0.1,
prediction_interval: 1.0 / 60.0,
min_output_rate: 180.0,
desired_number_of_samples: 20,
max_estimation_distance: 0.04,
min_travel_speed: 3.0,
max_travel_speed: 15.0,
max_linear_deviation: 0.2,
baseline_linearity_confidence: 0.4,
};
#[derive(Clone, Debug)]
struct AxisPredictor {
state: Vec4,
error_covariance: Mat4,
iterations: u32,
}
impl Default for AxisPredictor {
fn default() -> Self {
Self {
state: [0.0; 4],
error_covariance: identity(),
iterations: 0,
}
}
}
const TRANSITION: Mat4 = [
[1.0, 1.0, 0.5, 1.0 / 6.0],
[0.0, 1.0, 1.0, 0.5],
[0.0, 0.0, 1.0, 1.0],
[0.0, 0.0, 0.0, 1.0],
];
const PROCESS_NOISE_VECTOR: Vec4 = [1.0 / 6.0, 0.5, 1.0, 1.0];
impl AxisPredictor {
fn update(&mut self, observation: f64, params: &Params) {
self.iterations += 1;
if self.iterations == 1 {
self.state[0] = observation;
return;
}
self.state = mul_vec(&TRANSITION, &self.state);
self.error_covariance = add(
&mul(
&mul(&TRANSITION, &self.error_covariance),
&transpose(&TRANSITION),
),
&outer(
&PROCESS_NOISE_VECTOR,
&PROCESS_NOISE_VECTOR.map(|n| n * params.process_noise),
),
);
let y = observation - self.state[0];
let p = &self.error_covariance;
let s = p[0][0] + params.measurement_noise;
let gain: Vec4 = std::array::from_fn(|i| p[0][i] / s);
for (x, k) in self.state.iter_mut().zip(gain) {
*x += k * y;
}
let i_kh = add(
&identity(),
&outer(&gain.map(|k| -k), &[1.0, 0.0, 0.0, 0.0]),
);
self.error_covariance = add(
&mul(&mul(&i_kh, &self.error_covariance), &transpose(&i_kh)),
&outer(&gain, &gain.map(|k| k * params.measurement_noise)),
);
}
}
#[derive(Clone, Copy, Debug)]
struct State {
position: Vec2,
velocity: Vec2,
acceleration: Vec2,
jerk: Vec2,
}
fn evaluate_cubic(s: &State, dt: f64) -> State {
let (dt2, dt3) = (dt * dt, dt * dt * dt);
State {
position: [0, 1].map(|i| {
s.position[i]
+ s.velocity[i] * dt
+ s.acceleration[i] * dt2 / 2.0
+ s.jerk[i] * dt3 / 6.0
}),
velocity: [0, 1].map(|i| s.velocity[i] + s.acceleration[i] * dt + s.jerk[i] * dt2 / 2.0),
acceleration: [0, 1].map(|i| s.acceleration[i] + s.jerk[i] * dt),
jerk: s.jerk,
}
}
fn distance(a: Vec2, b: Vec2) -> f64 {
(a[0] - b[0]).hypot(a[1] - b[1])
}
fn normalize01(start: f64, end: f64, value: f64) -> f64 {
if start == end {
return if value > start { 1.0 } else { 0.0 };
}
((value - start) / (end - start)).clamp(0.0, 1.0)
}
fn interp(start: f64, end: f64, amount: f64) -> f64 {
start + (end - start) * amount.clamp(0.0, 1.0)
}
#[derive(Clone, Debug)]
pub struct KalmanPredictor {
params: Params,
x: AxisPredictor,
y: AxisPredictor,
sample_times: VecDeque<f64>,
last: Option<(f64, f64, f64)>,
}
impl Default for KalmanPredictor {
fn default() -> Self {
Self::with_params(DEFAULT)
}
}
impl KalmanPredictor {
pub fn with_params(params: Params) -> Self {
Self {
params,
x: AxisPredictor::default(),
y: AxisPredictor::default(),
sample_times: VecDeque::new(),
last: None,
}
}
fn estimated_state(&self) -> Option<State> {
let stable = self.params.min_stable_iteration;
if self.x.iterations < stable || self.y.iterations < stable {
return None;
}
let (Some(front), Some(back)) = (self.sample_times.front(), self.sample_times.back())
else {
return None;
};
let dt = (back - front) / self.sample_times.len().saturating_sub(1).max(1) as f64;
if dt <= 0.0 {
return None;
}
let w = &self.params;
let axis = |i: usize| [self.x.state[i], self.y.state[i]];
let scaled = |i: usize, by: f64| axis(i).map(|v| v * by);
Some(State {
position: axis(0),
velocity: scaled(1, 1.0 / dt),
acceleration: scaled(2, w.acceleration_weight / (dt * dt)),
jerk: scaled(3, w.jerk_weight / (dt * dt * dt)),
})
}
fn number_of_points(&self, estimated: &State, last: Vec2) -> usize {
let p = &self.params;
let target = p.prediction_interval * p.min_output_rate;
let sample_ratio =
(f64::from(self.x.iterations) / f64::from(p.desired_number_of_samples)).min(1.0);
let error = 1.0
- normalize01(
0.0,
p.max_estimation_distance,
distance(last, estimated.position),
);
let end = evaluate_cubic(estimated, p.prediction_interval);
let speed = distance(estimated.position, end.position) / p.prediction_interval;
let speed = normalize01(p.min_travel_speed, p.max_travel_speed, speed);
let linear =
[0, 1].map(|i| estimated.position[i] + p.prediction_interval * estimated.velocity[i]);
let deviation = distance(end.position, linear);
let linearity = interp(
p.baseline_linearity_confidence,
1.0,
1.0 - normalize01(0.0, p.max_linear_deviation, deviation),
);
let confidence = sample_ratio * error * speed * linearity;
(target * confidence).ceil().max(0.0) as usize
}
}
impl Predictor for KalmanPredictor {
fn reset(&mut self) {
self.x = AxisPredictor::default();
self.y = AxisPredictor::default();
self.sample_times.clear();
self.last = None;
}
fn update(&mut self, x: f64, y: f64, time: f64) {
if self
.last
.is_some_and(|(.., last)| time - last > MAX_TIME_DELTA)
{
self.reset();
}
self.last = Some((x, y, time));
self.sample_times.push_back(time / 1000.0);
if self.sample_times.len() > self.params.max_time_samples {
self.sample_times.pop_front();
}
self.x.update(x * CM_PER_PX, &self.params);
self.y.update(y * CM_PER_PX, &self.params);
}
fn predictions(&self) -> Vec<(f64, f64, f64)> {
let Some((x, y, time)) = self.last else {
return Vec::new();
};
let Some(estimated) = self.estimated_state() else {
return Vec::new();
};
let last = [x * CM_PER_PX, y * CM_PER_PX];
let n = self.number_of_points(&estimated, last);
let offset = [0, 1].map(|i| last[i] - estimated.position[i]);
let step = 1.0 / self.params.min_output_rate;
let mut state = estimated;
(1..=n)
.map(|k| {
state = evaluate_cubic(&state, step);
(
(state.position[0] + offset[0]) / CM_PER_PX,
(state.position[1] + offset[1]) / CM_PER_PX,
time + k as f64 * step * 1000.0,
)
})
.collect()
}
}
#[cfg(test)]
mod tests {
use super::super::chrome;
use super::*;
#[test]
fn a_steady_stroke_is_extrapolated_along_its_line() {
let mut p = KalmanPredictor::default();
let at = |t: f64| (10.0 + 0.5 * (t - 1000.0), 40.0 - 0.25 * (t - 1000.0));
for i in 0..40 {
let t = 1000.0 + f64::from(i) * 4.0;
let (x, y) = at(t);
p.update(x, y, t);
}
let last = 1000.0 + 39.0 * 4.0;
let predicted = p.predictions();
assert_eq!(predicted.len(), 3);
for (k, &(x, y, t)) in predicted.iter().enumerate() {
assert!((t - last - (k + 1) as f64 * 1000.0 / 180.0).abs() < 1e-9);
let (ex, ey) = at(t);
assert!(
(x - ex).abs() < 0.5 && (y - ey).abs() < 0.5,
"{x},{y} at {t}"
);
}
}
fn hook_past_end(p: &mut dyn Predictor) -> f64 {
let (length, duration) = (300.0, 400.0);
let x = |t: f64| {
let s = (t / duration).min(1.0);
length * s * s * s * (10.0 - 15.0 * s + 6.0 * s * s)
};
let mut hook: f64 = 0.0;
let mut frame = 1000.0 / 60.0;
for i in 1..120 {
let t = f64::from(i) * 1000.0 / 240.0;
let noise = 0.25 * (f64::from(i) * 2.399).sin();
p.update(x(t) + noise, 50.0 - noise, t);
if t >= frame {
frame += 1000.0 / 60.0;
for &(px, ..) in &p.predictions() {
hook = hook.max(px - length);
}
}
}
hook
}
#[test]
fn a_stroke_is_not_carried_past_where_it_ends() {
let ink = hook_past_end(&mut KalmanPredictor::default());
let chrome = hook_past_end(&mut chrome::KalmanPredictor::default());
assert!(ink < 1.0, "ink {ink}, chrome {chrome}");
assert!(chrome > 2.0, "ink {ink}, chrome {chrome}");
}
#[test]
fn a_pause_starts_over_and_needs_readings_again() {
let mut p = KalmanPredictor::default();
for i in 0..30 {
p.update(f64::from(i) * 4.0, 0.0, f64::from(i) * 8.0);
}
assert!(!p.predictions().is_empty());
p.update(200.0, 0.0, 300.0);
assert!(p.predictions().is_empty());
}
}