Задал вопрос: не изобретаем ли мы велосипед? Агент сделал обзор и ответил что в навигации нет, а в локализации да. Nav2 из ROS например не умеет разные радиусы поворота влево и вправо, так что свой Hybrid A* оставляем. А вся обвязка вокруг GEN_3 (таймауты, фильтр миллиметров, архивы карт, сбросы) это латание черного ящика, у которого одна поза одновременно и одометрия и глобальная привязка.
Первый шаг: разделил позу на odom и map. Навигация теперь едет по непрерывной одометрии, а прыжки карты ее не дергают. На проезде 2.8 м с поворотом расхождение 3 мм.