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. $(x_k-x_{k-1})/dt$ ${\bf H}$ ${\bf H} = [1\ \ \ 0]$ ${\bf x}_k$ $x_k$ $\dot{x}_{k}$

\begin{aligned} x_{k} & = (\begin{matrix} x_{k} \\ {\dot{x}}_{k} \end{matrix}) \end{aligned}

$\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 . Zatem jeśli pozycja i prędkość w czasie wynosiły i , a jeśli jest stałym przyspieszeniem, które stosuje się w przedziale czasu od do , z pomiaru możliwe jest wywnioskowanie wartość stosując wzór $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} d t + \frac{1}{2} a d t^{2}

$\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 d t = 2 \frac{{\hat{x}}_{k} - x_{k - 1}}{d t} - {\dot{x}}_{k - 1}

$\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{aligned} {\hat{x}}_{k} & = (\begin{matrix} {\hat{x}}_{k} \\ {\hat{\dot{x}}}_{k} \end{matrix}) \end{aligned}

$\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?

kalman-filters

— Stochastycznie
źródło

Nie przejrzałem wszystkich twoich obliczeń. Jednak mówiąc o przykładzie z Wikipedii, wydajesz się być nieco zdezorientowany co do jego struktury. Masz rację, że tylko pozycja jest mierzona. Jednak stosuje się tak zwany model „stałej prędkości”. Oznacza to, że prędkość jest uważana za stałą w macierzy przejścia stanu.

— Jason R

Zmiany prędkości modeluje się za pomocą macierzy szumów procesowych. Dlatego z założenia zakładasz, że prędkość zmieni się losowo z pewną określoną kowariancją. Zaskakujące jest to, że często działa dobrze. W ten sposób często stosuje się szum procesowy o jedną pochodną powyżej najwyższej pochodnej o zmiennej zmiennej stanu. Na przykład, jeśli włączyłeś przyspieszenie do swojego modelu, możesz mieć losowy komponent szarpnięcia zawarty w hałasie procesu.

— Jason R

@JasonR z modelem wikipedii (zakładając zerową kowariancję początkową między pozycją a prędkością), oszacowanie prędkości jest zawsze jej wartością początkową (jak mówisz, modelem „stałej prędkości”). Jednak wariancja prędkości rośnie monotonicznie przez hałas procesowy i nie ma żadnych pomiarów, które mogłyby ją zmniejszyć. Jaka jest przewaga tego nad modelem, który modeluje tylko pozycję i przyjmuje stałą prędkość?

— Stochastycznie

Wariancja oszacowania prędkości nie powinna wzrastać monotonicznie. Szum procesowy po prostu wprowadza składową stochastyczną do równania przejścia stanu, pozwalając wyrazić pewną niepewność co do tego, w jaki sposób stan systemu będzie ewoluował od czasu do czasu. Jeśli nie uwzględnisz szumu procesowego, twój filtr naprawdę będzie generował stałą prędkość. Prawdopodobnie nie tego chcesz.

— Jason R

Cóż, @JasonR, jeśli spojrzysz na model wikipedii, zobaczysz, że to monotynicznie rosnąca wariancja prędkości jest tym, co ci daje!

— Stochastycznie,

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{matrix} x_{k + 1} & = & x_{k} + {\dot{x}}_{k} Δ t + \frac{1}{2} a (Δ t)^{2} \\ {\dot{x}}_{k + 1} & = & {\dot{x}}_{k} + a Δ t \end{matrix}

$\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.

\begin{aligned} x_{k} & = (\begin{matrix} x_{k} \\ {\dot{x}}_{k} \end{matrix}) \end{aligned}

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

x_{k + 1} = (\begin{matrix} 1 Δ t \\ 0 1 \end{matrix}) x_{k} + (\begin{matrix} \frac{(Δ t)^{2}}{2} \\ Δ t \end{matrix}) a_{k}

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

$a_k$

To daje inną od poprzedniej wersji, ale macierze i powinny być takie same. $\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. $y$ $x_k$

wprowadź opis zdjęcia tutaj

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

wprowadź opis zdjęcia tutaj

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

wprowadź opis zdjęcia tutaj

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

wprowadź opis zdjęcia tutaj

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.

wprowadź opis zdjęcia tutaj

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

wprowadź opis zdjęcia tutaj

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 podaje $\mathbf{z}_{k+1}=x_{k+1}$ $\mathbf{P}_k=0$ $\mathbf{P}_{k+1}^{-}=\mathbf{Q}$ $\mathbf{K}_{k+1}$

K_{k + 1} = (\begin{matrix} 1 \\ 2 / d t \end{matrix})

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

Oznacza to, że powstaje procedura aktualizacji Kalmana

\begin{aligned} {\hat{x}}_{k + 1} & = F_{k + 1} x_{k} + K_{k + 1} (z_{k + 1} - H_{k + 1} F_{k + 1} x_{k}) \\ = (\begin{matrix} x_{k} + {\dot{x}}_{k} d t \\ {\dot{x}}_{k} \end{matrix}) + (\begin{matrix} 1 \\ 2 / d t \end{matrix}) (x_{k + 1} - (x_{k} + {\dot{x}}_{k} d t)) \\ = (\begin{matrix} x_{k + 1} \\ 2 (x_{k + 1} - x_{k}) / d t - {\dot{x}}_{k} \end{matrix}) \end{aligned}

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

— Peter K.
źródło

Dzięki za dotychczasową pomoc. Moje pierwotne pytanie zawierało pewne nieporozumienia, więc próbowałem je ponownie ukierunkować i próbowałem odpowiedzieć na twoje pytanie dotyczące z_k.

— Stochastycznie

dzięki vm za wszystkie twoje wysiłki :-). Twoja praca zachęciła mnie do zrobienia trochę matematyki, którą podjąłem do twojej odpowiedzi, mam nadzieję, że nie masz nic przeciwko. W każdym razie jestem teraz w 100% przekonany, że standardowa metoda jest dobra, ponieważ widzę tam formuły prędkości. Jeszcze raz dziękuję

— Stochastically

Cieszę się, że mogę pomóc! Nie ma problemu z dodaniem do odpowiedzi.

— Peter K.