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

Sintesi Tecnica: Un'estrazione dello Stato Ibrida EKF–FGO a Programmazione Intelligente

Definizione del Problema
L'affidabile stima dello stato nella robotica, in particolare all'interno delle pipeline di Simultaneous Localisation and Mapping (SLAM), richiede un compromesso tra accuratezza della stima e costo computazionale. I metodi basati sul filtraggio, come l'Extended Kalman Filter (EKF), offrono aggiornamenti in tempo reale efficienti ma soffrono di errori di linearizzazione che portano all'inconsistenza dello stimatore e all'accumulo di deriva nel tempo. Al contrario, gli approcci basati sull'ottimizzazione tramite Factor Graph Optimisation (FGO) migliorano la coerenza globale raffinando congiuntamente molteplici stati, ma comportano richieste computazionali significativamente più elevate. Sebbene le caratteristiche di questi singoli metodi siano ben note, l'organizzazione temporale della loro combinazione — specificamente, la programmazione di quando l'ottimizzazione globale debba essere invocata — ha ricevuto un'analisi sistematica limitata. In pratica, la programmazione dell'ottimizzazione è spesso trattata come una scelta di progettazione implicita piuttosto che come una variabile esplicita e regolabile.

Metodologia
Per affrontare questo vuoto, gli autori introducono un framework SSH (Smart Scheduled Hybrid) EKF–FGO. Questo framework è progettato per rendere esplicita ed esperimentalmente controllabile l'interazione tra filtraggio ricorsivo e ottimizzazione a blocchi (batch). La metodologia centrale prevede:

1. Architettura Ibrida: Il sistema esegue il filtraggio ricorsivo EKF ad ogni passo temporale per propagare le stime dello stato. Periodicamente, a intervalli discreti definiti da un parametro di programmazione $W$, viene attivata un'ottimizzazione a blocchi.
2. Scambio di Informazioni Bidirezionale:
   • Da EKF a FGO: Ad ogni evento di ottimizzazione, la stima EKF corrente (media $\mu_t$ e covarianza $P_t$) viene iniettata nel factor graph come un fattore di prior unary. Questa prior è pesata da un parametro di scala $\alpha$, consentendo al sistema di bilanciare l'aderenza alla convinzione (belief) del filtro rispetto all'ottimizzazione globale.
   • Da FGO a EKF: Dopo l'ottimizzazione a blocchi, l'EKF viene riinizializzato con le stime dello stato ottimizzate per mantenere la coerenza tra i componenti di filtraggio e ottimizzazione.
3. Controllo Sperimentale: Il framework fissa la struttura del solver (iterazioni di Gauss-Newton) e lo sforzo per isolare gli effetti dell'intervallo di programmazione $W$. Lo studio utilizza una simulazione SLAM planare con una traiettoria circolare per garantire condizioni geometriche coerenti per l'analisi degli effetti della programmazione.

Contributi Chiave
Il contributo primario di questo lavoro è la caratterizzazione sperimentale della programmazione dell'ottimizzazione come una dimensione di progettazione distinta nei sistemi di stima dello stato ibridi. Piuttosto che proporre un nuovo algoritmo di inferenza o un'architettura di solver, il documento fornisce un framework controllato per analizzare come la frequenza dell'ottimizzazione a blocchi influenzi:

• Il comportamento di stima intermedio (specificamente la deriva pre-ottimizzazione).
• Il costo computazionale.
• Lo spazio di compromesso tra filtraggio e ottimizzazione a blocchi.

Risultati
Esperimenti di simulazione che confrontano l'SSH EKF–FGO con un EKF puro e un classico baseline FGO "solo odometria" (dove la propagazione avviene senza filtraggio) rivelano diverse scoperte chiave:

• Riduzione della Deriva: L'approccio ibrido riduce significativamente la deriva pre-ottimizzazione rispetto alla propagazione basata solo sull'odometria. Attraverso intervalli di programmazione $W$ compresi tra 2 e 50, lo stimatore ibrido raggiunge una riduzione della deriva di circa l'80–90% rispetto al baseline. Anche a intervalli infrequenti (ad es., $W=200$), la deriva rimane di tre o quattro volte inferiore rispetto al baseline.
• Compromesso Asimmetrico: Lo studio identifica un'asimmetria critica nel comportamento del sistema:
  • Deriva: Aumenta gradualmente e monotonicamente all'aumentare dell'intervallo di programmazione $W$.
  • Costo Computazionale: Diminuisce rapidamente all'aumentare di $W$.
• Regimi Operativi: Questa asimmetria crea regimi operativi favorevoli (ad es., $W \in [10, 50]$) dove il sistema mantiene la maggior parte dei benefici di riduzione della deriva dell'ottimizzazione frequente, riducendo al contempo il carico computazionale di oltre un ordine di grandezza.
• Accuratezza Finale: L'errore assoluto della traiettoria (ATE) finale per lo stimatore ibrido rimane quasi costante attraverso tutti gli intervalli di programmazione e si avvicina strettamente all'accuratezza del puro FGO a blocchi, indicando che la programmazione influenza principalmente il comportamento intermedio piuttosto che la convergenza finale, a condizione che avvenga almeno un'ottimizzazione a blocchi.

Significatività
Il documento conclude che la programmazione dell'ottimizzazione è una dimensione di progettazione chiave, sebbene poco esplorata, nei sistemi di stima dello stato ibridi. Le scoperte dimostrano che, trattando esplicitamente la programmazione come un parametro regolabile, gli ingegneri possono navigare il compromesso tra accuratezza della stima ed efficienza computazionale in modo più efficace. Il framework SSH consente di mantenere la coerenza globale del livello FGO e la stabilità intermedia del livello EKF, permettendo al contempo sostanziosi risparmi computazionali. Gli autori pongono questo lavoro non come un sostituto dei consolidati solver incrementali (come iSAM2), ma come uno strumento per comprendere la dinamica fondamentale della programmazione nei sistemi ibridi, con lavori futuri pianificati per estendere tali analisi a strategie adattive e traiettorie più complesse.