Improve ball filter stability

Author: schmidma

crates/nodes/ball_filter/src/hypothesis.rs

@@ -89,7 +89,7 @@ impl BallHypothesis {
                     moving_process_noise,
                 );
 
-                let velocity_covariance = moving.covariance.fixed_view::<2, 2>(0, 0);
+                let velocity_covariance = moving.covariance.fixed_view::<2, 2>(2, 2);
                 let velocity = nalgebra::vector![moving.mean.z, moving.mean.w];
 
                 let exponent = -velocity.dot(
@@ -152,3 +152,33 @@ impl BallHypothesis {
         };
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn predict_keeps_precise_non_zero_velocity_moving() {
+        let mut hypothesis = BallHypothesis {
+            mode: BallMode::Moving(MultivariateNormalDistribution {
+                mean: nalgebra::vector![0.0, 0.0, 0.1, 0.0],
+                covariance: Matrix4::from_diagonal(&nalgebra::vector![
+                    0.005, 0.005, 0.000_001, 0.000_001
+                ]),
+            }),
+            last_seen: Time::zero(),
+            validity: 1.0,
+        };
+
+        hypothesis.predict(
+            Duration::ZERO,
+            Isometry2::identity(),
+            1.0,
+            Matrix4::zeros(),
+            Matrix2::zeros(),
+            0.5,
+        );
+
+        assert!(matches!(hypothesis.mode, BallMode::Moving(_)));
+    }
+}

crates/nodes/ball_filter/src/lib.rs

@@ -398,7 +398,7 @@ fn mahalanobis_matrix_of_hypotheses_and_percepts(
         let ball = hypothesis.position();
 
         let residual = percept.percept_in_ground.mean - ball.position.inner.coords;
-        let covariance = hypothesis.position_covariance();
+        let covariance = hypothesis.position_covariance() + percept.percept_in_ground.covariance;
 
         let mahalanobis_distance = residual.dot(
             &covariance
@@ -577,4 +577,31 @@ mod tests {
         assert_eq!(assignment.len(), 2);
         assert_eq!(assignment.into_iter().flatten().count(), 2);
     }
+
+    #[test]
+    fn matching_cost_accounts_for_percept_covariance() {
+        let hypothesis = BallHypothesis {
+            mode: BallMode::Moving(MultivariateNormalDistribution {
+                mean: nalgebra::vector![0.0, 0.0, 0.0, 0.0],
+                covariance: Matrix4::from_diagonal(&nalgebra::vector![0.01, 0.01, 1.0, 1.0]),
+            }),
+            last_seen: Time::zero(),
+            validity: 1.0,
+        };
+        let percept = BallPercept {
+            percept_in_ground: MultivariateNormalDistribution {
+                mean: vector![1.0, 0.0],
+                covariance: Matrix2::identity() * 100.0,
+            },
+            image_location: Circle::new(point![0.0, 0.0], 1.0),
+        };
+
+        let costs = mahalanobis_matrix_of_hypotheses_and_percepts(&[hypothesis], &[percept]);
+        let cost = costs[(0, 0)].into_inner();
+
+        assert!(
+            cost > -1.0,
+            "uncertain percept should not be treated as a precise outlier, got cost {cost}"
+        );
+    }
 }

crates/world_state/src/ball_filter.rs

@@ -16,7 +16,7 @@ use coordinate_systems::{Ground, Pixel};
 use framework::{AdditionalOutput, HistoricInput, MainOutput, PerceptionInput};
 use geometry::circle::Circle;
 use linear_algebra::{IntoFramed, Isometry2};
-use projection::{Projection, camera_matrix::CameraMatrix};
+use projection::{camera_matrix::CameraMatrix, Projection};
 use types::{
     ball_detection::BallPercept,
     ball_position::{BallPosition, HypotheticalBallPosition},
@@ -319,7 +319,7 @@ fn mahalanobis_matrix_of_hypotheses_and_percepts(
         let ball = hypothesis.position();
 
         let residual = percept.percept_in_ground.mean - ball.position.inner.coords;
-        let covariance = hypothesis.position_covariance();
+        let covariance = hypothesis.position_covariance() + percept.percept_in_ground.covariance;
 
         let mahalanobis_distance = residual.dot(
             &covariance
@@ -502,4 +502,31 @@ mod tests {
         assert_eq!(assignment.len(), 2);
         assert_eq!(assignment.into_iter().flatten().count(), 2);
     }
+
+    #[test]
+    fn matching_cost_accounts_for_percept_covariance() {
+        let hypothesis = BallHypothesis {
+            mode: BallMode::Moving(MultivariateNormalDistribution {
+                mean: nalgebra::vector![0.0, 0.0, 0.0, 0.0],
+                covariance: Matrix4::from_diagonal(&nalgebra::vector![0.01, 0.01, 1.0, 1.0]),
+            }),
+            last_seen: Time::zero(),
+            validity: 1.0,
+        };
+        let percept = BallPercept {
+            percept_in_ground: MultivariateNormalDistribution {
+                mean: vector![1.0, 0.0],
+                covariance: Matrix2::identity() * 100.0,
+            },
+            image_location: Circle::new(point![0.0, 0.0], 1.0),
+        };
+
+        let costs = mahalanobis_matrix_of_hypotheses_and_percepts(&[hypothesis], &[percept]);
+        let cost = costs[(0, 0)].into_inner();
+
+        assert!(
+            cost > -1.0,
+            "uncertain percept should not be treated as a precise outlier, got cost {cost}"
+        );
+    }
 }

etc/parameters/default.json

@@ -628,7 +628,7 @@
       "secs": 20
     },
     "log_likelihood_of_zero_velocity_threshold": 0.5,
-    "maximum_matching_cost": 0.25,
+    "maximum_matching_cost": 5.9914646,
     "maximum_matching_cost_validity_penalty_factor": 0.14,
     "maximum_number_of_hypotheses": 15,
     "noise": {