Kalman-Filter

Einfache Sprache

Der kalman-Filter ist eine Spezialisierung des Bayes-Filter. Der Belief $\text{Bel}(x_t)$ besteht aus einem Durchschnitt $\mu_t$ und der Kovarianzmatrix $\Sigma_t$. Jede Iteration benötigt der Algorithmus eine Aktion $u_t$ und eine Beobachtung $z_t$. Für Begriffserkärung siehe hier. Damit dier Kalman-Filter anwendbar ist müssen 3 Annahmen zusätzlich zur Markov Annahme gelten.

Ist nur für den kontinuerliche Räume definiert.

Sei $\mathfrak x_t =\left( \begin{array}{c} x_{1,t}\\\vdots\\x_{n,t}\end{array} \right)$ der Zustands-Vektor, $\mathfrak z_t =\left( \begin{array}{c} z_{1,t}\\\vdots\\ z_{k,t}\end{array} \right)$ der Mess-Vektor und $\mathfrak u_t =\left( \begin{array}{c} u_{1,t}\\\vdots\\x_{m,t}\end{array} \right)$ der Kontroll-Vektor bzw. Transitionsvektor.

Voraussetzungen

Folgende drei Eigenschaften müssen erfüllt sein, damit nach jeder Iteration $\text{Bel}(\mathfrak x_t)$ normalverteilt ist. Natürlich muss auch dieMarkov Annahme des Bayes-Filter erfüllt sein.

1. Linearität der Transitionsfunktion

Die Zustandsübergangswahrscheinlichkeit $p(\mathfrak x_t\mid \mathfrak u_t,\mathfrak x_{t-1})$ muss eine Lineare Funktion sein. Also es die Transitionsfunktion in Vektor-Schreibweise

$$\mathfrak x_t=\mathfrak A_t \mathfrak x_{t_1}+ \mathfrak B_t\mathfrak u_t +\mathfrak e_t\;,$$

welche die Bewegung des Roboters beschreibt. $\mathfrak e_t$ beschreibt die Unsicherheit der Transitionsfunktion und ist in der Regel normalverteilt mit Mittelwert $0$ und Kovarianzmatrix $\mathfrak R_t$. Daraus ergibt sich das der Mittelwert und die Kovarianz von $p(\mathfrak x_t\mid \mathfrak u_t,\mathfrak x_{t-1})$ durch $\mathfrak A_t \mathfrak x_{t_1}+ \mathfrak B_t\mathfrak u_t$ und $\mathfrak R_t$ definiert ist. Dies kann nach der Definition der Normalverteilung als

$$p(\mathfrak x_t\mid \mathfrak u_t,\mathfrak x_{t-1})= \text{det}(2\pi\mathfrak R_t)^{-1/2}\text{exp}\left(-\frac{1}{2}\left(\mathfrak x_t-\mathfrak A_t \mathfrak x_{t_1}- \mathfrak B_t\mathfrak u_t \right)^T\mathfrak R_t^{-1}\left(\mathfrak x_t-\mathfrak A_t \mathfrak x_{t_1}- \mathfrak B_t\mathfrak u_t \right)\right)$$

geschrieben werden.

2. Linearität der Wahrnehmungsfunktion

Auch $p(\mathfrak z_t\mid \mathfrak x_t)$ muss linear sein. Also durch eine Lineare Funktion dargestellt werden können

$$\mathfrak z_t = \mathfrak C_t\mathfrak x_t + \mathfrak d_t\;.$$

$\mathfrak d_t$ beschreibt die Unsicherheit der Wahrnehmungsfunktion und ist in der Regel normalverteilt mit Mittelwert $0$ und Kovarianzmatrix $\mathfrak Q_t$. Es ergibt sich wieder folgende Funktion

$$p(\mathfrak z_t\mid \mathfrak x_t)= \text{det}(2\pi\mathfrak Q_t)^{-1/2}\text{exp}\left(-\frac{1}{2}\left(\mathfrak z_t-\mathfrak C_t \mathfrak x_{t} \right)^T\mathfrak Q_t^{-1}\left(\mathfrak z_t-\mathfrak C_t \mathfrak x_{t} \right)\right)\;.$$

für die Berechnung.

3. normalvertielter Startbelief

Der Startbelief $\text{Bel}(\mathfrak x_0)$, bezüglich der realen Position $\mathfrak x_0$, muss normalverteilt mit Mittelwert $\mathfrak m_0$ und Kovarianz $\mathfrak S_0$. Also ist $\text{Bel}(\mathfrak x_0)$ gegeben durch

$$\begin{align}\text{Bel}(\mathfrak x_0) &= p(\mathfrak x_0)\\ &= \mathcal N(\mathfrak m_0,\mathfrak S_0) \\&= \text{det}(2\pi\mathfrak S_0)^{-1/2}\text{exp}\left(-\frac{1}{2}\left(\mathfrak x_0-\mathfrak m_0\right)^T\mathfrak S_0^{-1}\left(\mathfrak x_0-\mathfrak m_0\right)\right)\end{align}$$

Kalman gain

Der Kalman gain $\mathfrak K_t$ beschreibt zu welchem Grad die Messung in den neuen Zustand einfließt.

Innovation

Die Innovation im Kontext des Kalman-Filters beschreibt die Differenz zwischen tatsächlichen und erwarteten Messung, also $\mathfrak z_t -\mathfrak C_t \bar{\mathfrak m}_t$

Algorithmus

In Vektor-Schreibweise wird eine Iteration des Kalman-Filters durch folgenden Algorithmus berechnet:

	\begin{algorithm}
	\caption{Vektorisierter Kalman-Filter}
	\begin{algorithmic}
	\Input Mittelwertvektor $\mathfrak m_{t-1}$, Kovarianzmatrix $\mathfrak S_{t-1}$, Beobachtungsvektor $\mathfrak z_t$, Transitionsvektor $\mathfrak u_t$
	\Procedure{vectorized-kalman-filter}{$\mathfrak m_{t-1},\mathfrak S_{t-1}, \mathfrak z_t, \mathfrak u_t$}
	\State $\bar\mathfrak m_t \gets \mathfrak A_t\mathfrak m_{t-1}+\mathfrak B_t\mathfrak u_t$\Comment{Transformation.}
	\State $\bar\mathfrak S_t \gets \mathfrak A_t\mathfrak S_{t-1}\mathfrak A_t^T+ \mathfrak R_t$
	\State $\mathfrak K_t \gets \bar\mathfrak S_t\mathfrak C_{t}^T\left(\mathfrak C_t\bar\mathfrak S_t\mathfrak C^T_t+\mathfrak Q_t\right)^{-1}$\Comment{kalman gain.}
	\State $\mathfrak m_t \gets \bar\mathfrak m_t + \mathfrak K_t\left(\mathfrak z_t - \mathfrak C_t\bar\mathfrak m_t\right)$\Comment{Messung.}
	\State $\mathfrak S_t \gets \left(\mathfrak I - \mathfrak K_t\mathfrak C_t \right)\bar\mathfrak S_t$
	\Return $\mathfrak m_t,\mathfrak S_t$
    \EndProcedure
	\end{algorithmic}
	\end{algorithm}

Laufzeit durch $\mathcal O(n^2+k^{2.4})$ begrenzt. Herleitung in 3.2.4 in Probabilistic robotics.

Home: