Filtro Kalman per posizione e velocità: introduzione di stime di velocità

Grazie a tutti coloro che hanno pubblicato commenti / risposte alla mia domanda ieri ( Implementazione di un filtro Kalman per posizione, velocità, accelerazione ). Ho esaminato ciò che è stato raccomandato, e in particolare sia (a) l'esempio di Wikipedia su una posizione dimensionale e velocità, sia un altro sito Web che considera una cosa simile .

Aggiornamento 26-apr-2013 : la domanda originale qui conteneva alcuni errori, legati al fatto che non avevo capito bene l'esempio di Wikipedia su posizione e velocità monodimensionali . Con la mia migliore comprensione di ciò che sta accadendo, ora ho riformulato la domanda e la ho focalizzata più strettamente.

Entrambi gli esempi a cui mi riferisco nel paragrafo introduttivo sopra presuppongono che sia misurata solo la posizione. Tuttavia, nessuno dei due esempi ha alcun tipo di calcolo $(x_k-x_{k-1})/dt$ per la velocità. Ad esempio, l'esempio di Wikipedia specifica la matrice come , il che significa che viene inserita solo la posizione. Concentrandosi sull'esempio di Wikipedia, il vettore di stato del filtro Kalman contiene la posizione e la velocità , ovveroH = [ 1 0 ] x k x k ˙ x${\bf H}$${\bf H} = [1\ \ \ 0]$${\bf x}_k$$x_k$$\dot{x}_{k}$

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$

Supponiamo che la misurazione della posizione al momento sia . Quindi se la posizione e la velocità al momento erano e e se è un'accelerazione costante che si applica nell'intervallo di tempo da a , dalla misura di è possibile dedurre un valore per utilizzando la formulax k k - 1 x k - 1 ˙ x k - 1 a k - 1 k x$k$$\hat{x}_k$$k-1$$x_{k-1}$$\dot{x}_{k-1}$$a$$k-1$$k$$\hat{x}$$a$

$$\hat{x}_k = x_{k-1} + \dot{x}_{k-1} dt + \frac{1}{2} a dt^2$$

Ciò implica che al momento viene data una misura della velocità$k$$\hat{\dot{x}}_k$

$$\hat{\dot{x}}_k = \dot{x}_{k-1} + a dt = 2 \frac{\hat{x}_k - {x}_{k-1}}{dt} - \dot{x}_{k-1}$$

Tutte le quantità sul lato destro di tale equazione (ovvero , e ) sono normalmente variabili casuali distribuite con mezzi noti e deviazioni standard , quindi la matrice per il vettore di misurazione$\hat{x}_k$$x_{k-1}$$\dot{x}_{k-1}$$\bf R$

$$\begin{align*} \mathbf{\hat{x}}_{k} & =\left(\begin{array}[c]{c}\hat{x}_{k}\\ \hat{\dot{x}}_{k}\end{array} \right) \end{align*}$$

può essere calcolato. È un modo valido per introdurre stime della velocità nel processo?

È un modo valido per introdurre stime della velocità nel processo?

Se scegli il tuo stato in modo appropriato, le stime di velocità arrivano "gratuitamente". Vedi la derivazione del modello del segnale di seguito (per il semplice caso 1-D che abbiamo visto).

Signal Model, Take 2

Quindi, dobbiamo davvero concordare un modello di segnale prima di poter procedere in avanti. Dalla tua modifica, sembra che il tuo modello della posizione, , sia:$x_k$

$$\begin{array} xx_{k+1} &=& x_{k} + \dot{x}_{k} \Delta t + \frac{1}{2} a (\Delta t)^2\\ \dot{x}_{k+1} &=& \dot{x}_{k} + a \Delta t \end{array}$$

Se il nostro stato è come prima: quindi l'equazione di aggiornamento dello stato è solo: xk+1=( 1 Δ t 0 1 )xk+( ( Δ t )

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$ dove ora nostroakè l'accelerazione normalmente distribuita. $$\mathbf{x}_{k+1} = \left(\begin{array}[c]{c} 1\ \ \Delta t\\ 0\ \ 1\end{array} \right) \mathbf{x}_{k} + \left(\begin{array}[c]{c} \frac{(\Delta t)^2}{2} \\ \Delta t \end{array} \right) a_k$$ $a_k$

Ciò fornisce una matrice diversa dalla versione precedente, ma le matrici F e H dovrebbero essere le stesse.$\mathbf{G}$$\mathbf{F}$$\mathbf{H}$

---

Se lo implemento in scilab(scusate, nessun accesso a matlab), sembra che:

// Signal Model
DeltaT = 0.1;
F = [1 DeltaT; 0 1];
G = [DeltaT^2/2; DeltaT];
H = [1 0];

x0 = [0;0];
sigma_a = 0.1;

Q = sigma_a^2;
R = 0.1;

N = 1000;

a = rand(1,N,"normal")*sigma_a;

x_truth(:,1) = x0;
for t=1:N,
    x_truth(:,t+1) = F*x_truth(:,t) + G*a(t);
    y(t) = H*x_truth(:,t) + rand(1,1,"normal")*sqrt(R);
end

Quindi, posso applicare le equazioni del filtro Kalman a questo (le misurazioni rumorose).$y$

// Kalman Filter
p0 = 100*eye(2,2);

xx(:,1) = x0;
pp = p0;
pp_norm(1) = norm(pp);
for t=1:N,
    [x1,p1,x,p] = kalm(y(t),xx(:,t),pp,F,G,H,Q,R);
    xx(:,t+1) = x1;
    pp = p1;
    pp_norm(t+1) = norm(pp);
end

Quindi abbiamo le nostre misurazioni rumorose , e abbiamo applicato il filtro Kalman su di loro e abbiamo usato lo stesso modello di segnale per generare y come facciamo per applicare il filtro Kalman (un presupposto abbastanza grande, a volte!).$y$$y$

Quindi i seguenti grafici mostrano il risultato.

$y$$x_k$

Grafico 2 : una vista ingrandita dei primi esempi:

Trama 3 : qualcosa che non si ottiene mai nella vita reale, la posizione reale rispetto alla stima dello stato della posizione.

Trama 4 : qualcosa che non si ottiene mai nella vita reale, la vera velocità rispetto alla stima dello stato della velocità.

Trama 5 : la norma della matrice di covarianza statale (qualcosa che dovresti sempre monitorare nella vita reale!). Nota che passa rapidamente dal suo valore iniziale molto grande a qualcosa di molto piccolo, quindi ho mostrato solo i primi esempi.

Grafico 6 : grafici dell'errore tra la posizione e la velocità reali e le loro stime.

$\mathbf{z}_{k+1}=x_{k+1}$$\mathbf{P}_k=0$$\mathbf{P}_{k+1}^{-}=\mathbf{Q}$$\mathbf{K}_{k+1}$

$$\mathbf{K}_{k+1} = \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right)$$

Ciò significa che produce la procedura di aggiornamento di Kalman

$$\begin{align*} \mathbf{\hat{x}}_{k+1} & = \mathbf{F}_{k+1}\mathbf{x}_k + \mathbf{K}_{k+1}\left(\mathbf{z}_{k+1} - \mathbf{H}_{k+1} \mathbf{F}_{k+1}\mathbf{x}_k\right)\\ & = \left(\begin{array}[c]{c}x_k + \dot{x}_k dt\\ \dot{x}_k\end{array} \right) + \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right) \left(x_{k+1} - \left( x_k + \dot{x}_k dt\right) \right)\\ & = \left(\begin{array}[c]{c}x_{k+1}\\ 2 \left(x_{k+1} - x_k \right) /dt - \dot{x}_k\end{array} \right) \end{align*}$$

$(x_k-x_{k-1})/dt$