A Smart-Scheduled Hybrid (SSH) EKF-FGO State Estimation

Resumen Técnico: Una Estimación de Estado Híbrida EKF–FGO con Programación Inteligente

Planteamiento del Problema
La estimación de estado fiable en robótica, particularmente dentro de los procesos de Localización y Mapeo Simultáneos (SLAM), requiere un equilibrio entre la precisión de la estimación y el coste computacional. Los métodos basados en filtrado, como el Filtro de Kalman Extendido (EKF), ofrecen actualizaciones en tiempo real eficientes pero sufren de errores de linealización que conducen a la inconsistencia del estimador y a la acumulación de deriva con el tiempo. Por el contrario, los enfoques basados en optimización mediante la Optimización de Grafos de Factores (FGO) mejoran la consistencia global al refinar conjuntamente múltiples estados, pero conllevan demandas computacionales significativamente mayores. Si bien las características de estos métodos individuales están bien comprendidas, la organización temporal de su combinación —específicamente, la programación de cuándo debe invocarse la optimización global— ha recibido un análisis sistemático limitado. En la práctica, la programación de la optimización suele tratarse como una elección de diseño implícita en lugar de una variable explícita y ajustable.

Metodología
Para abordar esta brecha, los autores introducen un marco de trabajo EKF–FGO de Programación Inteligente (SSH, por sus siglas en inglés). Este marco está diseñado para hacer que la interacción entre el filtrado recursivo y la optimización por lotes sea explícita y experimentalmente controlable. La metodología central consiste en:

1. Arquitectura Híbrida: El sistema realiza un filtrado EKF recursivo en cada paso de tiempo para propagar las estimaciones de estado. Periódicamente, en intervalos discretos definidos por un parámetro de programación $W$, se activa una optimización por lotes.
2. Intercambio de Información Bidireccional:
   • EKF a FGO: En cada evento de optimización, la estimación actual del EKF (media $\mu_t$ y covarianza $P_t$) se inyecta en el grafo de factores como un factor de prioridad unaria. Esta prioridad está ponderada por un parámetro de escala $\alpha$, lo que permite al sistema equilibrar la adherencia a la creencia del filtro frente a la optimización global.
   • FGO a EKF: Tras la optimización por lotes, el EKF se reinicializa con las estimaciones de estado optimizadas para mantener la consistencia entre los componentes de filtrado y optimización.
3. Control Experimental: El marco fija la estructura del solver (iteraciones de Gauss-Newton) y el esfuerzo para aislar los efectos del intervalo de programación $W$. El estudio utiliza un SLAM planar con una trayectoria circular para asegurar condiciones geométricas consistentes para el análisis de los efectos de la programación.

Contribuciones Clave
La contribcción principal de este trabajo es la caracterización experimental de la programación de la optimización como una dimensión de diseño distinta en los sistemas de estimación de estado híbridos. En lugar de proponer un nuevo algoritmo de inferencia o una arquitectura de solver, el artículo proporciona un marco controlado para analizar cómo la frecuencia de la optimización por lotes influye en:

• El comportamiento de estimación intermedio (específicamente, la deriva previa a la optimización).
• El coste computacional.
• El espacio de compromiso entre el filtrado y la optimización por lotes.

Resultados
Los experimentos de simulación que comparan el SSH EKF–FGO contra un EKF puro y una línea base clásica de FGO de "solo odometría" (donde la propagación ocurre sin filtrado) revelan varios hallazgos clave:

• Reducción de la Deriva: El enfoque híbrido reduce significativamente la deriva previa a la optimización en comparación con la propagación de solo odometría. A través de intervalos de programación $W$ que van de 2 a 50, el estimador híbrido logra una reducción de la deriva de aproximadamente el 80–90% respecto a la línea base. Incluso en intervalos infrecuentes (por ejemplo, $W=200$), la deriva permanece tres o cuatro veces más baja que la de la línea base.
• Compromiso Asimétrico: El estudio identifica una asimetría crítica en el comportamiento del sistema:
  • Deriva: Aumenta de forma gradual y monótona a medida que aumenta el intervalo de programación $W$.
  • Coste Computacional: Disminuye rápidamente a medida que $W$ aumenta.
• Regímenes de Operación: Esta asimetría crea regímenes de operación favorables (por ejemplo, $W \in [10, 50]$) donde el sistema retiene la mayor parte de los beneficios de reducción de deriva de una optimización frecuente mientras reduce la carga computacional en más de un orden de magnitud.
• Precisión Final: El Error de Trayectoria Absoluta (ATE) final para el estimador híbrido permanece casi constante en todos los intervalos de programación y coincide estrechamente con la precisión del FGO de lote puro, lo que indica que la programación afecta principalmente al comportamiento intermedio en lugar de a la convergencia final, siempre que ocurra al menos una optimización por lotes.

Significancia
El artículo concluye que la programación de la optimización es una dimensión de diseño clave y, sin embargo, poco explorada en los sistemas de estimación de estado híbridos. Los hallazgos demuestran que, al tratar la programación como un parámetro ajustable, los ingenieros pueden navegar el compromiso entre la precisión de la estimación y la eficiencia computacional de manera más efectiva. El marco SSH permite retener la consistencia global de nivel FGO y la estabilidad intermedia de nivel EKF, permitiendo al mismo tiempo ahorros computacionales sustanciales. Los autores posicionan este trabajo no como un reemplazo para los solvers incrementales establecidos (como iSAM2), sino como una herramienta para comprender la dinámica fundamental de la programación en sistemas híbridos, con trabajos futuros planeados para extender estos análisis a estrategias adaptativas y trayectorias más complejas.