Stima della velocità dalla posizione nota e dall'accelerazione

Sono bloccato nel modellare un modello di sistema, ovvero ottenere il mio vettore di stato e il vettore di input. La mia ipotesi è che la posizione e la velocità siano il vettore di stato e l'accelerazione sia il vettore di input. La mia seconda ipotesi è che tutte e tre le quantità siano nel vettore di stato e nessuna nel vettore di input.

Quindi ... cos'è il vettore di stato e qual è il vettore di input nel mio caso?

Informazioni addizionali:

Ottengo misurazioni dal sensore di posizione e dal sensore di accelerazione. Tutto sta accadendo in 1D, ad esempio su una linea retta. Voglio unire queste letture (e rimuovere il rumore) per ottenere una stima della velocità per ogni intervallo di tempo.

Queste equazioni descrivono il sistema; Non sono sicuro però che siano modellati correttamente. Se comprendo correttamente è sicuro prevedere che l'accelerazione è costante (anche se in realtà cambia) - perché la matrice di covarianza del processo risolve questo assunto (giusto?). inserisci qui la descrizione dell'immagine

Ho anche alcuni dati di esempio con cui lavorare (i valori di input non sono riportati qui per semplicità):

 time    pos     acc      what I should get as output (velocity)
[0.0s]  0.000, -0.000  | 18.850
[0.1s]  1.885, -0.113  | 18.850
[0.2s]  3.768, -0.227  | 18.839
[0.3s]  5.650, -0.340  | 18.816
[0.4s]  7.528, -0.452  | 18.782
[0.5s]  9.401, -0.565  | 18.737

AGGIUNTA 2:

Per una migliore comunicazione sto creando una nuova risposta, ma dovrebbe essere trattato come un commento alla prima risposta. Jason mi hai già aiutato moltissimo e sono davvero grato per il tuo tempo. Tuttavia, ho ancora problemi con questo: i risultati di Kalman Filter non sono quelli previsti. Potresti trovare il tempo di leggere quanto segue, grazie. Ti devo già una birra o due (o casse se vuoi) - se hai paypal contattami su primoz [at] codehunter.eu :)

Ho implementato il modello che Jason aveva proposto nella prima risposta. Ho aggiunto il cretino come quarta variabile di stato. Dopo ore di revisione ho deciso di tornare qui per chiedere aiuto. I valori che ottengo da KF non sono quelli previsti. La tabella seguente rappresenta i dati delle prime 10 iterazioni dell'algoritmo. Notare come il jerk aumenta ogni volta che si sbagliano le altre stime. Dopo un secondo la differenza tra l'accelerazione reale e quella stimata è superiore a 1m / s² (vedi tabella, ultima riga)!

           real           measured                   estimated                    real 
time   pos     acc       pos    acc        pos      acc    jerk    vel[!]       velocity 
0.0   0.000  -0.000    -0.040  0.030  |  -0.300   -0.060   0.000   18.850  <-->  18.850
0.1   1.885  -0.113     1.965 -0.153  |   1.585   -0.061  -0.006   18.844  <-->  18.844
0.2   3.768  -0.227     3.778 -0.247  |   3.469   -0.066  -0.035   18.835  <-->  18.827
0.3   5.650  -0.340     5.750 -0.370  |   5.351   -0.090  -0.122   18.815  <-->  18.799
0.4   7.528  -0.452     7.358 -0.452  |   7.228   -0.152  -0.291   18.769  <-->  18.759
0.5   9.401  -0.565     9.251 -0.555  |   9.094   -0.282  -0.574   18.673  <-->  18.708
0.6   11.269 -0.677    11.309 -0.717  |   10.938  -0.518  -1.006   18.494  <-->  18.646
0.7   13.130 -0.788    13.260 -0.758  |   12.752  -0.840  -1.490   18.233  <-->  18.573
0.8   14.983 -0.899    15.043 -0.949  |   14.520  -1.286  -2.096   17.854  <-->  18.488
0.9   16.827 -1.009    16.977 -1.089  |   16.235  -1.838  -2.770   17.362  <-->  18.393
1.0   18.661 -1.118    18.831 -1.168  |   17.890  -2.477  -3.476   16.762  <-->  18.287

Le mie matrici sono qui:

Cosa sta causando questa aggiunta in ogni timestep per jerk ? Qualcuno delle mie matrici è sbagliato?

Lo stesso vale per la prima soluzione (solo modello a 3 stati): l'accelerazione non cambia come dovrebbe.

ULTIMA MODIFICA:

Finalmente sono riuscito a farlo funzionare. Non sono sicuro che ci sia stato un errore di implementazione o matrici P&Q sbagliate.

kalman-filters

— Primož Kralj
fonte

Per ottenere una risposta utile è necessario fornire ulteriori informazioni. Descrivi il sistema. Mostraci quali equazioni hai messo insieme.

— Jim Clay,

Grazie per la vostra risposta. Ho fornito ulteriori informazioni.

— Primož Kralj,

Per favore qualcuno mi aiuti in questo - mi sento come se stessi perdendo la testa.

— Primož Kralj,

Non vedo alcun problema evidente. È probabile che tu abbia un errore di implementazione.

— Jason R,

@Phonon, nessun sentimento duro. Mi sento come se fossi stato sulle montagne russe :)

— Primož Kralj,

Risposte:

In questo caso non assumeresti un'accelerazione costante. In genere lo faresti se non avessi i mezzi per misurare l'accelerazione del sistema, ma dici che è osservabile per il tuo caso. Il modo più ovvio per modellare questo sistema sarebbe usare il vettore di stato

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \end{matrix}]

$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \end{array} \right ]$

dove è la posizione, è la velocità e è l'accelerazione del sistema all'istante . Dato che dici che puoi misurare la posizione e l'accelerazione del sistema, il vettore di misurazione sarebbe: $x_k$ $v_k$ $a_k$ $k$ $\mathbf{z_k}$

z_{k} = H x_{k} + v_{k}

$\mathbf{z_k} = \mathbf{Hx_k} + \mathbf{v_k}$

con

H = [\begin{array}{cc} 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}]

$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 \\ 0 && 0 && 1 \end{array} \right]$

risultante nel modello di misurazione:

z_{k} = [\begin{matrix} x_{k} \\ a_{k} \end{matrix}] + v_{k}

$\mathbf{z_k} = \left[ \begin{array}{c} x_k \\ a_k \end{array} \right] + \mathbf{v_k}$

dove è il rumore (gaussiano) inerente alla misurazione. Ora, a meno che non si stia aggiungendo una forza nota al sistema che influirebbe sulla sua dinamica, allora il vettore di input . Vedi questa recente domanda per una discussione più dettagliata dei vari termini nel modello di Kalman, che devi conoscere e che no . $\mathbf{v_k}$ $\mathbf{u_k} = \mathbf{0}$

Modifica: per quanto riguarda il commento di seguito su come gestire la matrice di transizione di stato per il termine di accelerazione: ci sono un paio di modi diversi per gestirlo. Si potrebbe supporre un modello a jerk zero (jerk è la derivata del tempo dell'accelerazione); questo equivale a supporre che . Oppure, potresti aggiungere un quarto elemento al tuo vettore di stato: $a_{k+1}=a_k$

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \\ \overset{⃛}{x_{k}} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \\ j_{k} \end{matrix}]

$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \\ \dddot{x_k} \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \\ j_k \end{array} \right ]$

dove è il jerk del sistema, o velocità temporale di variazione dell'accelerazione, all'istante . Poiché non stai misurando direttamente il jerk, avresti una matrice di misurazione di: $j_k$ $k$

H = [\begin{array}{cc} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}]

$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 && 0\\ 0 && 0 && 1 && 0 \end{array} \right]$

E la matrice di transizione di stato diventerebbe:

F = [\begin{array}{cccc} 1 & d t & \frac{1}{2} d t^{2} & \frac{1}{6} d t^{3} \\ 0 & 1 & d t & \frac{1}{2} d t^{2} \\ 0 & 0 & 1 & d t \\ 0 & 0 & 0 & 1 \end{array}]

$\mathbf{F} = \left[ \begin{array}{cccc} 1 && dt && \frac{1}{2}dt^2 && \frac{1}{6} dt^3 \\ 0 && 1 && dt && \frac{1}{2}dt^2 \\ 0 && 0 && 1 && dt \\ 0 && 0 && 0 && 1 \end{array} \right]$

Questo è un modello "jerk costante" (simile ai modelli a velocità costante o ad accelerazione costante che potresti vedere in semplici esempi); la tua matrice di transizione di stato presuppone implicitamente che il jerk sia costante per tutti i valori di k. Questo non è probabilmente vero. Per gestire quell'aspetto del problema, è necessario introdurre un termine di rumore di processo nella componente jerk dell'equazione di transizione dello stato.

Qualitativamente, questo ti permette di esprimere che non sei sicuro di come cambierà il termine jerk di volta in volta, ma che ti aspetti che le modifiche siano casuali con una certa distribuzione gaussiana. Questo è uno strumento spesso usato per "mettere a punto" modelli di filtri adattivi come questo fino a trovare un set di parametri che funzioni bene per la tua applicazione.

— Jason R
fonte

Grazie, mi hai davvero acceso una luce in testa. Ma non riesco ancora a capire quale sarà la mia accelerazione prevista nel prossimo passaggio (se non costante) - questo è importante per costruire la matrice di transizione A, l'ultima riga per essere precisi. Che cosa suggeriresti?

— Primož Kralj,

C'è un bel casino con le domande di migrazione. Parte della tua risposta è andata. Spero che le mod lo riparino. Inoltre, guarda Addition 2 nella mia domanda originale.

— Primož Kralj,

Sono stato in grado di recuperare il testo modificato e lo ho riapplicato alla mia risposta.

— Jason R,

Potresti spiegare come ottenere la covarianza dei processi per la prima soluzione (3 variabili di stato)? Non riesco a trovare un link decente online che lo spieghi in pratica.

— Primož Kralj,

Suppongo che le tue variabili di stato siano conformi alla risposta di Jason. In tal caso, credo che il tuo problema potrebbe essere la tua matrice (rumore di processo): poiché è zero ovunque tranne che per l'aggiornamento jerk, gli unici termini che aggiornano i valori di posizione, velocità e accelerazione sono le parti deterministiche del aggiorna le equazioni (eccetto dove la natura stocastica del jerk passa). $Q$

Prova a usare una matrice come: $Q$

Q = [\begin{array}{cccc} 0.00001 & 0 & 0 & 0 \\ 0 & 0.00001 & 0 & 0 \\ 0 & 0 & 0.00001 & 0 \\ 0 & 0 & 0 & 0.001 \end{array}]

$Q = \left [ \begin{array}{cccc} 0.00001 & 0& 0& 0\\ 0 & 0.00001 & 0 & 0\\ 0 & 0 & 0.00001 & 0 \\ 0 & 0& 0& 0.001 \end{array} \right ]$

Se quegli elementi diagonali sono zero, il modello sta dicendo "Non mi aspetto che la posizione, la velocità o l'accelerazione cambino (tranne che dalle equazioni di aggiornamento)" --- che chiaramente non è il caso.

— Peter K.
fonte

Bella spiegazione - ma perché scegliere 0,001, perché non 2,84 per esempio?

— Primož Kralj,

Grazie! Il motivo della scelta tra 0,00001 e 2,84: Beh, 2,84 è MOLTO grande per una varianza. Supponevo che tu avessi un ragionamento valido dietro il numero jerk (0,001) e che gli altri numeri dovrebbero essere significativamente più piccoli di questo. Se non esiste un motivo reale per la scelta del numero jerk, provare lo stesso valore in tutti gli elementi diagonali.

— Peter K.

Ma cosa significa in pratica? Se dico che il mio sensore di posizione ha una varianza di 0,00001 (primo elemento diagonale nella matrice Q), ciò significa che l'errore è di + - 0,00001 metri per esempio (suppongo di no)? Immagino che tu scelga una varianza minore per un sensore più preciso (posizione) e una varianza maggiore per un sensore con meno precisione (accelerazione).

— Primož Kralj,

Se dici che la varianza di una misura è , allora nel framework del filtro Kalman, stai dicendo che è gaussiana distribuita con una deviazione standard di . Pertanto, ~ 68% delle volte, l'entità dell'errore di misurazione sarà inferiore a , ~ 95% delle volte, sarà inferiore a e così via.

0.00001

$0.00001$

\sqrt{0.00001}

$\sqrt{0.00001}$

\sqrt{0.00001}

$\sqrt{0.00001}$

2 \sqrt{0.00001}

$2\sqrt{0.00001}$

— Jason R,

Grazie ad entrambi, spero che questa discussione possa aiutare anche i futuri visitatori.

— Primož Kralj,

Penso che il problema sia in Q. Q stanno caricando K con i loro valori. Ottengo buoni risultati quando Q è precaricato con poteri di delta_t.

Vedi qualsiasi articolo sul modello di salciccia. Ha una buona matrice Q.

A00 deve essere multiplo della potenza delta_t di 5 A11 potenza delta_t di 3 A22 potenza delta_t di 1 A01 multiplo della potenza delta_t di 4

E così via

Guarda a

https://www.google.com/url?sa=t&source=web&rct=j&url=https://pdfs.semanticscholar.org/9c1b/db6b5a25c1231e775c654f731e1ca1fef31a.pdf&ved=2ahUKEwj4vJbU1qLlAhUWAmMBHe7-DYcQFjADegQIBRAB&usg=AOvVaw1ELb4Ql84HAJot2rzktVsI

— Jm Rams
fonte