Ball filter - Not so Smart Update (HULKs#1253)

* match balls using kuhn-munkres

* --wip--

* --wip--

* use loglikelihood to get confidence of zero velocity

* use correct parameter

* remove unused parameter

* aaaaaarrghhhhhhhhhhhh

* validity tuning

* impl merging

* increase measurement noise in distance

* add noise increase in distance

* tune parameters

* fix ci

* tune validity decrease after non matching hypothesis

* add merge criterion

* fix cargo files

* remove unneeded output

* make factor a parameter

* remove unused variable + import

* remove removed

* tune log_likelihood

* fix home version

* format tomls

---------

Co-authored-by: okiwi6 <oleflb>

Cargo.toml

@@ -1,59 +1,115 @@
 [workspace]
 members = [
   "crates/aliveness",
+  "crates/aliveness",
+  "crates/approx_derive",
   "crates/approx_derive",
   "crates/argument_parsers",
+  "crates/argument_parsers",
+  "crates/audio",
   "crates/audio",
   "crates/ball_filter",
+  "crates/ball_filter",
+  "crates/bevyhavior_simulator",
   "crates/bevyhavior_simulator",
   "crates/buffered_watch",
+  "crates/buffered_watch",
   "crates/calibration",
+  "crates/calibration",
+  "crates/code_generation",
   "crates/code_generation",
   "crates/communication",
+  "crates/communication",
+  "crates/constants",
   "crates/constants",
   "crates/context_attribute",
+  "crates/context_attribute",
+  "crates/control",
   "crates/control",
   "crates/coordinate_systems",
+  "crates/coordinate_systems",
   "crates/energy_optimization",
+  "crates/energy_optimization",
+  "crates/filtering",
   "crates/filtering",
   "crates/framework",
+  "crates/framework",
+  "crates/geometry",
   "crates/geometry",
   "crates/hardware",
+  "crates/hardware",
+  "crates/hulk",
   "crates/hulk",
   "crates/hulk_imagine",
+  "crates/hulk_imagine",
   "crates/hulk_manifest",
+  "crates/hulk_manifest",
+  "crates/hulk_nao",
   "crates/hulk_nao",
   "crates/hulk_replayer",
+  "crates/hulk_replayer",
   "crates/hulk_webots",
+  "crates/hulk_webots",
+  "crates/hulk_widgets",
   "crates/hulk_widgets",
+  "crates/hungarian_algorithm",
+  "crates/hungarian_algorithm",
+  "crates/kinematics",
   "crates/kinematics",
   "crates/linear_algebra",
+  "crates/linear_algebra",
+  "crates/motionfile",
   "crates/motionfile",
   "crates/nao",
+  "crates/nao",
+  "crates/nao_camera",
   "crates/nao_camera",
   "crates/object_detection",
+  "crates/object_detection",
+  "crates/opn",
   "crates/opn",
   "crates/parameter_tester",
+  "crates/parameter_tester",
   "crates/parameters",
+  "crates/parameters",
+  "crates/path_serde",
   "crates/path_serde",
   "crates/path_serde_derive",
+  "crates/path_serde_derive",
+  "crates/projection",
   "crates/projection",
   "crates/ransac",
+  "crates/ransac",
+  "crates/repository",
   "crates/repository",
   "crates/source_analyzer",
+  "crates/source_analyzer",
   "crates/spl_network",
+  "crates/spl_network",
+  "crates/spl_network_messages",
   "crates/spl_network_messages",
   "crates/types",
+  "crates/types",
+  "crates/vision",
   "crates/vision",
   "crates/walking_engine",
+  "crates/walking_engine",
+  "tools/annotato",
   "tools/annotato",
   "tools/camera_matrix_extractor",
+  "tools/camera_matrix_extractor",
   "tools/depp",
+  "tools/depp",
+  "tools/fanta",
   "tools/fanta",
   "tools/hula/types",
+  "tools/hula/types",
   "tools/pepsi",
+  "tools/pepsi",
+  "tools/twix",
   "tools/twix",
   "tools/widget_gallery",
+  "tools/widget_gallery",
 ]
 resolver = "2"
 # HuLA and Aliveness are built independently by yocto
@@ -118,11 +174,12 @@ geometry = { path = "crates/geometry" }
 gilrs = "0.10.1"
 glob = "0.3.0"
 hardware = { path = "crates/hardware" }
-home = "0.5.4"
+home = "=0.5.9"
 hula-types = { path = "tools/hula/types" }
 hulk = { path = "crates/hulk" }
 hulk_manifest = { path = "crates/hulk_manifest" }
 hulk_widgets = { path = "crates/hulk_widgets" }
+hungarian_algorithm = { path = "crates/hungarian_algorithm" }
 i2cdev = "0.5.1"
 image = "0.24.4"
 indicatif = "0.17.2"
@@ -160,6 +217,7 @@ parameters = { path = "crates/parameters" }
 parking_lot = "0.12.1"
 path_serde = { path = "crates/path_serde" }
 path_serde_derive = { path = "crates/path_serde_derive" }
+pathfinding = "<=4.10.0"
 petgraph = "0.6.2"
 png = "0.17.6"
 prettyplease = "0.2.19"

crates/ball_filter/Cargo.toml

@@ -10,6 +10,7 @@ coordinate_systems = { workspace = true }
 filtering = { workspace = true }
 linear_algebra = { workspace = true }
 nalgebra = { workspace = true }
+ordered-float = { workspace = true }
 path_serde = { workspace = true }
 serde = { workspace = true }
 types = { workspace = true }

crates/ball_filter/src/hypothesis.rs

@@ -1,4 +1,7 @@
-use std::time::{Duration, SystemTime};
+use std::{
+    f32::consts::PI,
+    time::{Duration, SystemTime},
+};
 
 use filtering::kalman_filter::KalmanFilter;
 use moving::{MovingPredict, MovingUpdate};
@@ -17,104 +20,132 @@ use types::{
 pub mod moving;
 pub mod resting;
 
+#[derive(Clone, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect)]
+pub enum BallMode {
+    Resting(MultivariateNormalDistribution<2>),
+    Moving(MultivariateNormalDistribution<4>),
+}
+
 #[derive(Clone, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect)]
 pub struct BallHypothesis {
-    pub moving: MultivariateNormalDistribution<4>,
-    pub resting: MultivariateNormalDistribution<2>,
+    pub mode: BallMode,
     pub last_seen: SystemTime,
     pub validity: f32,
 }
 
 impl BallHypothesis {
-    pub fn new(
-        moving_hypothesis: MultivariateNormalDistribution<4>,
-        resting_hypothesis: MultivariateNormalDistribution<2>,
-        last_seen: SystemTime,
-    ) -> Self {
+    pub fn new(hypothesis: MultivariateNormalDistribution<4>, last_seen: SystemTime) -> Self {
         Self {
-            moving: moving_hypothesis,
-            resting: resting_hypothesis,
+            mode: BallMode::Moving(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 position(&self) -> BallPosition<Ground> {
+        match self.mode {
+            BallMode::Resting(resting) => BallPosition {
+                position: resting.mean.framed().as_point(),
+                velocity: Vector2::zeros(),
+                last_seen: self.last_seen,
+            },
+            BallMode::Moving(moving) => BallPosition {
+                position: moving.mean.xy().framed().as_point(),
+                velocity: vector![moving.mean.z, moving.mean.w],
+                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 position_covariance(&self) -> Matrix2<f32> {
+        match self.mode {
+            BallMode::Resting(resting) => resting.covariance,
+            BallMode::Moving(moving) => moving.covariance.fixed_view::<2, 2>(0, 0).into_owned(),
         }
     }
 
-    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,
+        log_likelihood_of_zero_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;
+        match &mut self.mode {
+            BallMode::Resting(resting) => {
+                RestingPredict::predict(resting, last_to_current_odometry, resting_process_noise)
+            }
+            BallMode::Moving(moving) => {
+                MovingPredict::predict(
+                    moving,
+                    delta_time,
+                    last_to_current_odometry,
+                    velocity_decay,
+                    moving_process_noise,
+                );
+
+                let velocity_covariance = moving.covariance.fixed_view::<2, 2>(0, 0);
+                let velocity = nalgebra::vector![moving.mean.z, moving.mean.w];
+
+                let exponent = -velocity.dot(
+                    &velocity_covariance
+                        .cholesky()
+                        .expect("covariance not invertible")
+                        .solve(&velocity),
+                ) / 2.;
+                let determinant = velocity_covariance.determinant();
+
+                let log_likelihood_of_zero_velocity =
+                    exponent - (2. * PI * determinant.sqrt()).ln();
+
+                if log_likelihood_of_zero_velocity > log_likelihood_of_zero_velocity_threshold {
+                    self.mode = BallMode::Resting(MultivariateNormalDistribution {
+                        mean: moving.mean.xy(),
+                        covariance: moving.covariance.fixed_view::<2, 2>(0, 0).into_owned(),
+                    })
+                }
+            }
         }
     }
 
     pub fn update(
         &mut self,
         detection_time: SystemTime,
         measurement: MultivariateNormalDistribution<2>,
+        validity_bonus: f32,
     ) {
         self.last_seen = detection_time;
-        MovingUpdate::update(&mut self.moving, measurement);
-        RestingUpdate::update(&mut self.resting, measurement);
-        self.validity += 1.0;
+        self.validity += validity_bonus;
+
+        match &mut self.mode {
+            BallMode::Resting(resting) => RestingUpdate::update(resting, measurement),
+            BallMode::Moving(moving) => MovingUpdate::update(moving, measurement),
+        }
     }
 
     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,
-        );
+        match (&mut self.mode, other.mode) {
+            (BallMode::Resting(resting), BallMode::Resting(distribution)) => {
+                KalmanFilter::update(
+                    resting,
+                    Matrix2::identity(),
+                    distribution.mean,
+                    distribution.covariance,
+                );
+                self.validity = self.validity.max(other.validity);
+            }
+            (BallMode::Moving(moving), BallMode::Moving(distribution)) => {
+                KalmanFilter::update(
+                    moving,
+                    Matrix4::identity(),
+                    distribution.mean,
+                    distribution.covariance,
+                );
+                self.validity = self.validity.max(other.validity);
+            }
+            _ => (), // deny merge
+        };
     }
 }