Turned the robot off for the night, but before that dumped all the day's data into Parquet, and the agent dug through it overnight without the robot. It found a great one: for a few seconds after relocalization the ZED SDK returns coordinates in millimeters, even though the settings say meters. 21 576 such poses in a day. That's where the 4 kilometer "jumps" in the logs came from, and a broken map with obstacles exactly where the robot had just driven. Also the KNOWN_MAP status flips back to INITIALIZING 5-8 seconds later, so "found itself" now only counts after 10 seconds of stable status.
The filter turned out simple: if the pose jumps further than the robot could have driven, it goes into quarantine for 4 seconds. If it comes back, it was a glitch. If it stays at the new place, it's a real correction.
def update(self, now, position):
if self.accepted is None or self.reachable(self.accepted, now, position):
self.accepted = (now, position)
self.pending = None
return True
if self.pending is not None and self.reachable(self.pending[1:], now, position):
if now - self.pending[0] >= self.settle_s:
self.accepted = (now, position)
return True
return False
self.pending = (now, now, position)
return False
