refactor ball filter (#1044)

--wip--

extract ball_filter to crate

integrate review comments

change time ordered retrieval

get rid of all that traits

flatten and default

fix twix and spawning

make clippy happy

Co-authored-by: okiwi6 <oleflb>

Cargo.toml

@@ -5,6 +5,7 @@ members = [
   "crates/approx_derive",
   "crates/argument_parsers",
   "crates/audio",
+  "crates/ball_filter",
   "crates/buffered_watch",
   "crates/calibration",
   "crates/code_generation",
@@ -70,6 +71,7 @@ approx_derive = { path = "crates/approx_derive" }
 argument_parsers = { path = "crates/argument_parsers" }
 audio = { path = "crates/audio" }
 awaitgroup = "0.6.0"
+ball_filter = { path = "crates/ball_filter" }
 base64 = "0.21.0"
 bat = { version = "0.23.0", default-features = false, features = [
   "regex-onig",

crates/ball_filter/Cargo.toml

@@ -0,0 +1,15 @@
+[package]
+name = "ball_filter"
+version.workspace = true
+edition.workspace = true
+license.workspace = true
+homepage.workspace = true
+
+[dependencies]
+coordinate_systems = { workspace = true }
+filtering = { workspace = true }
+linear_algebra = { workspace = true }
+nalgebra = { workspace = true }
+path_serde = { workspace = true }
+serde = { workspace = true }
+types = { workspace = true }

crates/ball_filter/src/ball_position.rs

@@ -0,0 +1,14 @@
+use std::time::SystemTime;
+
+use linear_algebra::{Point2, Vector2};
+use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
+use serde::{Deserialize, Serialize};
+
+#[derive(
+    Debug, Clone, Copy, PathDeserialize, PathSerialize, PathIntrospect, Serialize, Deserialize,
+)]
+pub struct BallPosition<Frame> {
+    pub position: Point2<Frame>,
+    pub velocity: Vector2<Frame>,
+    pub last_seen: SystemTime,
+}

crates/ball_filter/src/hypothesis.rs

@@ -0,0 +1,121 @@
+use std::time::{Duration, SystemTime};
+
+use filtering::kalman_filter::KalmanFilter;
+use moving::{MovingPredict, MovingUpdate};
+use nalgebra::{Matrix2, Matrix4};
+use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
+use resting::{RestingPredict, RestingUpdate};
+use serde::{Deserialize, Serialize};
+
+use coordinate_systems::Ground;
+use linear_algebra::{vector, IntoFramed, Isometry2, Point2, Vector2};
+
+use types::multivariate_normal_distribution::MultivariateNormalDistribution;
+
+use crate::ball_position::BallPosition;
+
+pub mod moving;
+pub mod resting;
+
+#[derive(Clone, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect)]
+pub struct BallHypothesis {
+    moving: MultivariateNormalDistribution<4>,
+    resting: MultivariateNormalDistribution<2>,
+    last_seen: SystemTime,
+    pub validity: f32,
+}
+
+impl BallHypothesis {
+    pub fn new(
+        moving_hypothesis: MultivariateNormalDistribution<4>,
+        resting_hypothesis: MultivariateNormalDistribution<2>,
+        last_seen: SystemTime,
+    ) -> Self {
+        Self {
+            moving: moving_hypothesis,
+            resting: resting_hypothesis,
+            last_seen,
+            validity: 1.0,
+        }
+    }
+
+    pub fn resting(&self) -> BallPosition<Ground> {
+        BallPosition {
+            position: self.resting.mean.xy().framed().as_point(),
+            velocity: Vector2::zeros(),
+            last_seen: self.last_seen,
+        }
+    }
+
+    pub fn moving(&self) -> BallPosition<Ground> {
+        BallPosition {
+            position: self.moving.mean.xy().framed().as_point(),
+            velocity: vector![self.moving.mean.z, self.moving.mean.w],
+            last_seen: self.last_seen,
+        }
+    }
+
+    pub fn choose_ball(&self, velocity_threshold: f32) -> BallPosition<Ground> {
+        let moving = self.moving();
+        if moving.velocity.norm() < velocity_threshold {
+            return self.resting();
+        };
+        moving
+    }
+
+    pub fn predict(
+        &mut self,
+        delta_time: Duration,
+        last_to_current_odometry: Isometry2<Ground, Ground>,
+        velocity_decay: f32,
+        moving_process_noise: Matrix4<f32>,
+        resting_process_noise: Matrix2<f32>,
+        velocity_threshold: f32,
+    ) {
+        MovingPredict::predict(
+            &mut self.moving,
+            delta_time,
+            last_to_current_odometry,
+            velocity_decay,
+            moving_process_noise,
+        );
+        RestingPredict::predict(
+            &mut self.resting,
+            last_to_current_odometry,
+            resting_process_noise,
+        );
+
+        let moving_velocity: Vector2<Ground> = vector![self.moving.mean.z, self.moving.mean.w];
+        if moving_velocity.norm() < velocity_threshold {
+            self.resting.mean.x = self.moving.mean.x;
+            self.resting.mean.y = self.moving.mean.y;
+        }
+    }
+
+    pub fn update(
+        &mut self,
+        detection_time: SystemTime,
+        measurement: Point2<Ground>,
+        noise: Matrix2<f32>,
+    ) {
+        self.last_seen = detection_time;
+        MovingUpdate::update(&mut self.moving, measurement, noise);
+        RestingUpdate::update(&mut self.resting, measurement, noise);
+        self.validity += 1.0;
+    }
+
+    pub fn merge(&mut self, other: BallHypothesis) {
+        KalmanFilter::update(
+            &mut self.moving,
+            Matrix4::identity(),
+            other.moving.mean,
+            other.moving.covariance,
+        );
+        KalmanFilter::update(
+            &mut self.resting,
+            Matrix2::identity(),
+            other.resting.mean,
+            other.resting.covariance,
+        );
+    }
+}

crates/ball_filter/src/hypothesis/moving.rs

@@ -0,0 +1,65 @@
+use std::time::Duration;
+
+use coordinate_systems::Ground;
+use filtering::kalman_filter::KalmanFilter;
+use linear_algebra::{Isometry2, Point2};
+use nalgebra::{matrix, Matrix2, Matrix2x4, Matrix4, Matrix4x2};
+use types::multivariate_normal_distribution::MultivariateNormalDistribution;
+
+pub(super) trait MovingPredict {
+    fn predict(
+        &mut self,
+        delta_time: Duration,
+        last_to_current_odometry: Isometry2<Ground, Ground>,
+        velocity_decay: f32,
+        process_noise: Matrix4<f32>,
+    );
+}
+
+pub(super) trait MovingUpdate {
+    fn update(&mut self, measurement: Point2<Ground>, noise: Matrix2<f32>);
+}
+
+impl MovingPredict for MultivariateNormalDistribution<4> {
+    fn predict(
+        &mut self,
+        delta_time: Duration,
+        last_to_current_odometry: Isometry2<Ground, Ground>,
+        velocity_decay: f32,
+        process_noise: Matrix4<f32>,
+    ) {
+        let dt = delta_time.as_secs_f32();
+        let constant_velocity_prediction = matrix![
+            1.0, 0.0, dt, 0.0;
+            0.0, 1.0, 0.0, dt;
+            0.0, 0.0, velocity_decay, 0.0;
+            0.0, 0.0, 0.0, velocity_decay;
+        ];
+
+        let rotation = last_to_current_odometry.inner.rotation.to_rotation_matrix();
+        let rotation = rotation.matrix();
+        let translation = last_to_current_odometry.inner.translation.vector;
+
+        let state_rotation = matrix![
+            rotation.m11, rotation.m12, 0.0, 0.0;
+            rotation.m21, rotation.m22, 0.0, 0.0;
+            0.0, 0.0, rotation.m11, rotation.m12;
+            0.0, 0.0, rotation.m21, rotation.m22;
+        ];
+
+        let state_prediction = constant_velocity_prediction * state_rotation;
+        KalmanFilter::predict(
+            self,
+            state_prediction,
+            Matrix4x2::identity(),
+            translation,
+            process_noise,
+        );
+    }
+}
+
+impl MovingUpdate for MultivariateNormalDistribution<4> {
+    fn update(&mut self, measurement: Point2<Ground>, noise: Matrix2<f32>) {
+        KalmanFilter::update(self, Matrix2x4::identity(), measurement.inner.coords, noise)
+    }
+}

crates/ball_filter/src/hypothesis/resting.rs

@@ -0,0 +1,42 @@
+use coordinate_systems::Ground;
+use filtering::kalman_filter::KalmanFilter;
+use linear_algebra::{Isometry2, Point2};
+use nalgebra::Matrix2;
+use types::multivariate_normal_distribution::MultivariateNormalDistribution;
+
+pub(super) trait RestingPredict {
+    fn predict(
+        &mut self,
+        last_to_current_odometry: Isometry2<Ground, Ground>,
+        process_noise: Matrix2<f32>,
+    );
+}
+
+pub(super) trait RestingUpdate {
+    fn update(&mut self, measurement: Point2<Ground>, noise: Matrix2<f32>);
+}
+
+impl RestingPredict for MultivariateNormalDistribution<2> {
+    fn predict(
+        &mut self,
+        last_to_current_odometry: Isometry2<Ground, Ground>,
+        process_noise: Matrix2<f32>,
+    ) {
+        let rotation = last_to_current_odometry.inner.rotation.to_rotation_matrix();
+        let translation = last_to_current_odometry.inner.translation.vector;
+
+        KalmanFilter::predict(
+            self,
+            *rotation.matrix(),
+            Matrix2::identity(),
+            translation,
+            process_noise,
+        );
+    }
+}
+
+impl RestingUpdate for MultivariateNormalDistribution<2> {
+    fn update(&mut self, measurement: Point2<Ground>, noise: Matrix2<f32>) {
+        KalmanFilter::update(self, Matrix2::identity(), measurement.inner.coords, noise)
+    }
+}