From 8dad71a63c0a0cff25afae7ca6a957672e29d848 Mon Sep 17 00:00:00 2001 From: oliverw10 Date: Thu, 31 Mar 2022 19:34:29 +1100 Subject: [PATCH] detect lost target and stop gating innovation --- components/vision.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/components/vision.py b/components/vision.py index 00584a7..aecee7d 100644 --- a/components/vision.py +++ b/components/vision.py @@ -78,6 +78,7 @@ def __init__(self) -> None: self.target_yaw = 0.0 self.last_latency = 0.0 self.timestamp = 0.0 + self.discarded_count = 0 def setup(self) -> None: self.field_obj = self.field.getObject("vision_pose") @@ -162,8 +163,12 @@ def execute(self) -> None: self.chassis.estimator.getEstimatedPosition().translation() ) # Gate on innovation - if self.gate_innovation and innovation > 2.0: - return + if self.gate_innovation and innovation > 3.0: + self.discarded_count += 1 + if self.discarded_count < 100: + return + else: + self.discarded_count = 0 if self.distance < 5.0: std_dev = 0.3 * self.max_std_dev