La posizione multipla stima la fusione

Ho un sistema in cui ho due sottosistemi separati per la stima delle posizioni dei robot. Il primo sottosistema è composto da 3 telecamere che vengono utilizzate per rilevare i marker che il robot sta trasportando e che genera 3 stime della posizione e dell'orientamento del robot. Il secondo sottosistema è un sistema che si trova sul robot e sta misurando la velocità sui due punti del robot. Integrando numericamente quei due posso ottenere una stima della posizione e dell'orientamento del robot (perché sto seguendo due punti contemporaneamente).

Il primo sistema è meno preciso ma il secondo sistema va alla deriva. Il primo sistema produce circa una volta al secondo, mentre il secondo produce molto più frequentemente (100-200 volte al secondo).

Presumo che ci debba essere un approccio migliore rispetto al semplice ripristino della posizione con la stima del primo sistema (in quanto non è accurata al 100%), ma di utilizzare anche la posizione accumulata dal secondo sistema di sensori e fondere quella con i nuovi dati dal primo sistema. Inoltre, c'è una domanda su come fondere 3 stime del primo sistema? Deve esserci un modo migliore della media pura in quanto potrebbe accadere che le due stime siano esattamente le stesse e la terza sia completamente diversa (nel senso che probabilmente è più sbagliata)?

Hai qualche algoritmo di fusione da raccomandare di usare in un tale sistema? Conosco il filtro Kalman, ma ho difficoltà a capire come usarlo come i due sistemi trasmettono dati a frequenze diverse.

Spero che la domanda sia abbastanza chiara, qual è l'approccio migliore per fondere le stime in una stima più corretta e accurata?

Grazie

Quello che stai descrivendo è essenzialmente un caso da manuale per l'uso di un filtro Kalman. Innanzitutto è necessario un passaggio di previsione. Supponiamo che tu stia predicendo la posa del robot$(x,y,\theta)$, data la stima della posa precedente e le misurazioni della velocità ad alta frequenza $(v,\omega)$, dove $v$ è la velocità lineare e $\omega$ è la velocità angolare.

$P$ è la matrice di covarianza 3x3 che rappresenta l'incertezza della posa del robot. $Q$ è la covarianza dei tuoi input (cioè, quanto sono rumorose quelle misurazioni di velocità?) $F$ è il giacobino del modello di movimento rispetto allo stato e $G$ è il giacobino rispetto agli input, cioè

Ora hai i tuoi aggiornamenti di correzione meno frequenti, che misurano effettivamente lo stato completo, rendendolo abbastanza semplice, cioè

dove $z_k$ è la tua misurazione (dalla fotocamera) e $R$è la matrice di covarianza associata a quella misurazione (probabilmente una matrice diagonale). Questa misurazione viene confrontata con la misurazione prevista (che nel tuo caso è solo l'ultima stima della posa). In questo semplice caso, il guadagno di Kalman è la proporzione della covarianza di posa corrente rispetto alla somma della covarianza di posa e della covarianza di misurazione.

Per rispondere alla tua domanda sulle diverse tariffe, puoi semplicemente eseguire ripetutamente l'aggiornamento di movimento fino all'arrivo dell'aggiornamento di previsione. Ad esempio, potrebbe accadere che l'aggiornamento del movimento avvenga 100 volte prima di eseguire una correzione.

Hai anche chiesto come gestire tre telecamere. Il modo più semplice è semplicemente elaborarli in sequenza; applica solo tre correzioni di fila. Un altro modo è quello di impilarli ed eseguire un singolo aggiornamento. Dovresti modificare la fase di aggiornamento della correzione per farlo in questo modo.