Filtr Kalmana dla pozycji i prędkości: wprowadzenie szacunków prędkości

Dziękuję wszystkim, którzy opublikowali wczoraj komentarze / odpowiedzi na moje zapytanie ( Wdrożenie filtru Kalmana dla pozycji, prędkości, przyspieszenia ). Patrzyłem na to, co było zalecane, a w szczególności na (a) przykład Wikipedii na temat jednowymiarowej pozycji i prędkości, a także na innej stronie internetowej, która rozważa podobną rzecz .

Aktualizacja 26 kwietnia 2013 r . : pierwotne pytanie zawierało pewne błędy związane z faktem, że nie zrozumiałem poprawnie przykładu wikipedii na temat jednowymiarowej pozycji i prędkości . Dzięki mojemu lepszemu zrozumieniu tego, co się dzieje, teraz przeredagowałem pytanie i skoncentrowałem je bardziej precyzyjnie.

Oba przykłady, o których mowa w powyższym akapicie wstępnym, zakładają, że mierzona jest tylko pozycja. Jednak żaden przykład nie ma żadnego rodzaju obliczeń dla prędkości. Na przykład w Wikipedii podano macierz jako , co oznacza, że ​​wprowadzana jest tylko pozycja. Koncentrując się na przykładzie Wikipedii, wektor stanu filtru Kalmana zawiera pozycję i prędkość , tj.H H = [ 1 0 ] x k x k ˙ x$(x_k-x_{k-1})/dt$${\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*}$$

Załóżmy, że miarą pozycji w czasie jest \ hat {x} _k . Zatem jeśli pozycja i prędkość w czasie k-1 wynosiły x_ {k-1} i \ dot {x} _ {k-1} , a jeśli a jest stałym przyspieszeniem, które stosuje się w przedziale czasu od k-1 do k , z pomiaru \ hat {X} możliwe jest wywnioskowanie wartość stosując wzórx k k - 1 x k - 1 ˙ x k - 1 k - $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$$

Oznacza to, że w chwili $k$ podaje się pomiar $\hat{\dot{x}}_k$ prędkości

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

Wszystkie wielkości po prawej stronie tego równania (tj. $\hat{x}_k$ , $x_{k-1}$ i $\dot{x}_{k-1}$ ) są zwykle rozkładowymi zmiennymi losowymi o znanych średnich i odchyleniach standardowych , więc macierz $\bf R$ dla wektora pomiarowego

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

można obliczyć. Czy jest to prawidłowy sposób wprowadzania szacunków prędkości do procesu?

Czy jest to prawidłowy sposób wprowadzania szacunków prędkości do procesu?

Jeśli odpowiednio wybierzesz swój stan, prognozy prędkości będą dostępne „za darmo”. Zobacz wyprowadzenie modelu sygnału poniżej (dla prostego przypadku 1-D, na który patrzyliśmy).

Model sygnału, weź 2

Tak więc naprawdę musimy uzgodnić model sygnału, zanim będziemy mogli przejść do przodu. Z twojej edycji wygląda na to, że twój model pozycji, , to:$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}$$

Jeśli nasz stan jest taki jak przedtem: to równanie aktualizacji stanu jest po prostu: gdzie teraz jest normalnie rozproszonym przyspieszeniem. 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*}$$ a $$\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$

To daje inną od poprzedniej wersji, ale macierze i powinny być takie same.F $\mathbf{G}$$\mathbf{F}$$\mathbf{H}$

---

Jeśli zaimplementuję to w scilab(przepraszam, brak dostępu do Matlaba), wygląda to tak:

// 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

Następnie można zastosować równań filtru Kalmana do tego (hałaśliwe pomiarów).$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

Mamy więc nasze głośne pomiary i zastosowaliśmy do nich filtr Kalmana i użyliśmy tego samego modelu sygnału do wygenerowania tak jak robimy to, aby zastosować filtr Kalmana (czasami całkiem duże założenie!).$y$$y$

Następnie kolejne wykresy pokazują wynik.

Działka 1 : i w funkcji czasu.x $y$$x_k$

Wykres 2 : Powiększony widok pierwszych kilku próbek:

Fabuła 3 : Coś, czego nigdy nie dostaniesz w prawdziwym życiu, prawdziwa pozycja w porównaniu z oceną stanu pozycji.

Fabuła 4 : Coś, czego nigdy nie uzyskasz w prawdziwym życiu, prawdziwa prędkość w porównaniu z estymacją stanu prędkości.

Wykres 5 : Norma macierzy kowariancji stanu (coś, co zawsze należy monitorować w prawdziwym życiu!). Zauważ, że bardzo szybko zmienia swoją początkową bardzo dużą wartość na coś bardzo małego, więc pokazałem tylko kilka pierwszych próbek.

Wykres 6 : Wykresy błędu między prawdziwą pozycją a prędkością oraz ich oszacowania.

Jeśli przestudiujesz przypadek, w którym pomiary pozycji są dokładne, okaże się, że równania udman Kalmana dają dokładne wyniki ZARÓWNO pozycji i prędkości. Matematycznie łatwo jest zrozumieć, dlaczego. Używając tego samego zapisu co w artykule na Wikipedii , dokładne pomiary oznaczają, że . Jeśli przyjmiesz, że początkowa pozycja i prędkość są znane, więc , to i macierz wzmocnienia Kalmana podajeP k =0 P - k + 1 = Q K k + $\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)$$

Oznacza to, że powstaje procedura aktualizacji Kalmana

$$\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*}$$

Jak widać, wartość prędkości jest podana dokładnie przez formułę, którą proponujesz zastosować do oszacowania prędkości. Więc chociaż nie widziałeś żadnego rodzaju obliczeń dla prędkości, w rzeczywistości jest tam ukryty.$(x_k-x_{k-1})/dt$