Wesentliche Grundlagen II (Essential background II)

Bevor wir den mehrdimensionalen Kalman-Filter behandeln, müssen wir einige wesentliche mathematische Themen wiederholen:

• Matrixoperationen (matrix operations)
• Erwartungsalgebra (expectation algebra)
• Multivariate Normalverteilung (multivariate normal distribution)

Wenn Sie mit diesen Themen vertraut sind, können Sie direkt zum nächsten Kapitel springen.

Die in diesem Tutorial verwendete Notation:

• Fettdruck, Kleinbuchstaben beziehen sich auf Vektoren (vectors), z. B. \( \boldsymbol{x} \)
• Fettdruck, Großbuchstaben beziehen sich auf Matrizen (matrices), z. B. \( \boldsymbol{A} \)
• Normale Schrift, Kleinbuchstaben beziehen sich auf Skalare (scalars) oder Vektorelemente
• Normale Schrift, Großbuchstaben beziehen sich auf Matrixelemente

Beispielorientierter Leitfaden zum Kalman-Filter

Zum Buch

Matrixoperationen

Sie müssen nur die grundlegenden Begriffe und Operationen kennen, wie zum Beispiel:

• Addition und Multiplikation von Vektoren und Matrizen
• Transponieren von Matrizen (matrix transpose)
• Matrixinverse (matrix inverse) (Sie müssen Matrizen nicht selbst invertieren; Sie müssen nur wissen, was die Inverse einer Matrix ist.)
• Symmetrische Matrizen (symmetric matrices)
• Eigenwerte und Eigenvektoren (eigenvalues and eigenvectors)

Es gibt zahlreiche Lehrbücher und Web-Tutorials zur Linearen Algebra (linear algebra), die diese Themen behandeln.

Erwartungsalgebra

Ich verwende die Regeln der Erwartungsalgebra bei der Herleitung der Kalman-Filter-Gleichungen sehr häufig. Wenn Sie die Herleitungen verstehen möchten, sollten Sie die Erwartungsalgebra beherrschen.

Sie wissen bereits, was eine Zufallsvariable (random variable) ist und was ein Erwartungswert (expected value) bzw. eine Erwartung (expectation) ist. Falls nicht, lesen Sie bitte den Abschnitt Wesentliche Grundlagen I.

Grundlegende Erwartungsregeln

Der Erwartungswert wird mit dem Großbuchstaben \( E \) bezeichnet.

Der Erwartungswert der Zufallsvariablen \( E(X) \) ist gleich dem Mittelwert (mean) der Zufallsvariablen:

Hier sind einige grundlegende Erwartungsregeln:

Erwartungsregeln für Varianz und Kovarianz

Die folgende Tabelle enthält die Erwartungsregeln für Varianz und Kovarianz.

Die Erwartungsregeln für Varianz und Kovarianz sind nicht unmittelbar ersichtlich. Einige davon beweise ich hier.

Regel 8

\[ V(a) = 0 \]

Eine Konstante variiert nicht, daher ist die Varianz einer Konstante gleich 0.

Regel 9

\[ V(a \pm X) = V(X) \]

Das Addieren einer Konstante zu einer Variablen ändert deren Varianz nicht.

Regel 10

\[ V(X) = E(X^{2}) - \mu_{\scriptscriptstyle \!X}^{2} \]

Der Beweis:

Regel 11

\[ COV(X,Y) = E(XY) - \mu_{\scriptscriptstyle \!X}\mu_{\scriptscriptstyle \!Y} \]

Der Beweis:

Regel 13

\[ V(aX) = a^{2}V(X) \]

Der Beweis:

Für eine Bewegung mit konstanter Geschwindigkeit:

Regel 14

\[ V(X \pm Y) = V(X) + V(Y) \pm 2COV(X,Y) \]

Der Beweis:

Multivariate Normalverteilung

Einführung

Wir haben gesehen, dass der Output des Kalman-Filters eine Zufallsvariable (random variable) ist. Der Mittelwert (mean) der Zufallsvariablen entspricht der Zustandsschätzung. Die Varianz (variance) der Zufallsvariablen ist die Schätzunsicherheit. Der Kalman-Filter liefert uns somit sowohl die Schätzung als auch das zugehörige Konfidenzniveau.

Die eindimensionalen Kalman-Filter-Gleichungen enthalten vier Unsicherheitsgrößen:

• \( p_{n,n} \) ist die Varianz einer Schätzung (der aktuelle Zustand)
• \( p_{n+1,n} \) ist die Varianz einer Vorhersage (der nächste Zustand)
• \( r_{n} \) ist die Messvarianz
• \( q \) ist das Prozessrauschen (process noise)

Beim mehrdimensionalen Kalman-Filter wird der Systemzustand durch einen Vektor mit mehr als einer Variable beschrieben. Zum Beispiel kann die Position eines Objekts in einer Ebene durch zwei Variablen beschrieben werden: x – Position und y – Position:

Beispielorientierter Leitfaden zum Kalman-Filter

Zum Buch

\[ \boldsymbol{x} = \left[ \begin{matrix} x\\ y\\ \end{matrix} \right] \]

Der Output des Kalman-Filters ist eine mehrdimensionale Zufallsvariable (multivariate random variable). Eine Kovarianzmatrix (covariance matrix) beschreibt die quadrierte Unsicherheit dieser mehrdimensionalen Zufallsvariablen.

Die Unsicherheitsgrößen des mehrdimensionalen Kalman-Filters sind:

• \( \boldsymbol{P}_{n,n} \) ist eine Kovarianzmatrix, die die quadrierte Unsicherheit einer Schätzung beschreibt
• \( \boldsymbol{P}_{n+1,n} \) ist eine Kovarianzmatrix, die die quadrierte Unsicherheit einer Vorhersage beschreibt
• \( \boldsymbol{R}_{n} \) ist eine Kovarianzmatrix, die die quadrierte Messunsicherheit beschreibt
• \( \boldsymbol{Q} \) ist eine Kovarianzmatrix, die das Prozessrauschen beschreibt

In diesem Kapitel geht es um die multivariate Normalverteilung (multivariate normal distribution) und die Kovarianzmatrix.

Kovarianz

Die Kovarianz ist ein Maß für die Stärke der Korrelation zwischen zwei oder mehr Mengen von Zufallsvariablen.

Betrachten wir eine Menge von Positionsmessungen eines Objekts in der \( x\text{-}y \)-Ebene.

Aufgrund des Zufallsfehlers gibt es eine Streuung (Varianz) in den Messungen.

Schauen wir uns einige Beispiele unterschiedlicher Messreihen an.

Die beiden oberen Teilplots zeigen unkorrelierte Messungen. Die \( x \)-Werte hängen nicht von den \( y \)-Werten ab. Im blauen Datensatz haben die \( x \)- und \( y \)-Werte die gleiche Varianz; die Verteilung ist kreisförmig. Im roten Datensatz ist die Varianz der \( x \)-Werte größer als die der \( y \)-Werte; die Verteilung ist elliptisch.

Da die Messungen unkorreliert sind, ist die Kovarianz von \( x \) und \( y \) gleich Null.

Die beiden unteren Teilplots zeigen korrelierte Messungen. Zwischen \( x \)- und \( y \)-Werten besteht eine Abhängigkeit. Im grünen Datensatz führt ein Anstieg von \( x \) zu einem Anstieg von \( y \) (und umgekehrt). Die Korrelation ist positiv, daher ist die Kovarianz positiv. Im türkisen Datensatz führt ein Anstieg von \( x \) zu einem Abfall von \( y \) (und umgekehrt). Die Korrelation ist negativ, daher ist die Kovarianz negativ.

Die Kovarianz zwischen Grundgesamtheit \( X \) und Grundgesamtheit \( Y \) mit der Größe \( N \) lautet:

\[ COV(X,Y) = \frac{1}{N}\sum_{i=1}^{N}(x_{i} - \mu_{x})(y_{i} - \mu_{y}) \]

Schreiben wir die Kovarianzgleichung um:

Die Kovarianz einer Stichprobe (sample) der Größe \( N \) wird mit \( N-1 \) normiert:

\[ COV(X,Y) = \frac{1}{N-1}\sum_{i=1}^{N}(x_{i} - \mu_{x})(y_{i} - \mu_{y}) \]

Schreiben wir die Kovarianzgleichung um:

Beispiel:

Gegebene Stichproben:

\[ \boldsymbol{x} = \left[ \begin{matrix} 2 \\ 3 \\ -1 \\ 4 \end{matrix} \right] \]

\[ \boldsymbol{y} = \left[ \begin{matrix} 8 \\ 7 \\ 9 \\ 6 \end{matrix} \right] \]

\[ COV(X,Y) = \frac{1}{N-1}\sum_{i=1}^{N}(x_{i}y_{i}) - \frac{N}{N-1}\mu_{x}\mu_{y} = \]

\[ = \frac{1}{3} \left( 2 \times 8 + 3 \times 7 - 1 \times 9 + 4 \times 6 \right) - \frac{4}{3} \left( \frac{(2+3-1+4)}{4} \frac{(8+7+9+6)}{4} \right) = -2.67 \]

Wir können die Stichproben in zwei Vektoren \( \boldsymbol{x} \) und \( \boldsymbol{y} \) zusammenfassen. Die Kovarianz in Vektornotation lautet:

\[ COV(X,Y) = \frac{1}{N-1}\boldsymbol{x}^{T}\boldsymbol{y} - \frac{N}{N-1}\mu_{x}\mu_{y} \]

Für eine Zufallsvariable mit Mittelwert Null (zero-mean random variable) ergibt sich die Kovarianz zu:

\[ COV(X,Y) = \frac{1}{N-1}\boldsymbol{x}^{T}\boldsymbol{y} \]

Kovarianzmatrix

Eine Kovarianzmatrix (covariance matrix) ist eine quadratische Matrix, die die Kovarianz zwischen jedem Paar von Elementen einer gegebenen mehrdimensionalen Zufallsvariablen darstellt.

Für eine zweidimensionale Zufallsvariable lautet die Kovarianzmatrix:

\[ \boldsymbol{\Sigma} = \left[ \begin{matrix} \sigma_{xx} & \sigma_{xy} \\ \sigma_{yx} & \sigma_{yy} \\ \end{matrix} \right] = \left[ \begin{matrix} \sigma_{x}^{2} & \sigma_{xy} \\ \sigma_{yx} & \sigma_{y}^{2} \\ \end{matrix} \right] = \left[ \begin{matrix} VAR(\boldsymbol{x}) & COV(\boldsymbol{x, y}) \\ COV(\boldsymbol{y, x}) & VAR(\boldsymbol{y}) \\ \end{matrix} \right] \]

Beachten Sie, dass die Nebendiagonaleinträge der Kovarianzmatrix gleich sind, da \( COV(\boldsymbol{x, y}) = COV(\boldsymbol{y, x}) \). Wenn \( x \) und \( y \) unkorreliert sind, sind die Nebendiagonaleinträge der Kovarianzmatrix Null.

Für eine \( n \)-dimensionale Zufallsvariable lautet die Kovarianzmatrix:

\[ \boldsymbol{\Sigma} = \left[ \begin{matrix} \sigma_{1}^{2} & \sigma_{12} & \cdots & \sigma_{1n} \\ \sigma_{21} & \sigma_{2}^{2} & \cdots & \sigma_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ \sigma_{n1} & \sigma_{n2} & \cdots & \sigma_{n}^{2}\\ \end{matrix} \right] \]

Die meisten wissenschaftlichen Softwarepakete können die Kovarianzmatrix berechnen.

import numpy as np

x = np.array([2, 3, -1, 4])
y = np.array([8, 7, 9, 6])

C = np.cov(x,y)
print(C)

[[ 4.66666667 -2.66666667]
 [-2.66666667  1.66666667]]
                    

x = [2 3 -1 4];
y = [8 7 9 6];

C = cov(x,y)

C =

    4.6667   -2.6667
   -2.6667    1.6667
                    

Eigenschaften der Kovarianzmatrix

1. Die Diagonaleinträge dieser Kovarianzmatrix sind die Varianzen der Komponenten der mehrdimensionalen Zufallsvariablen.

\[ \boldsymbol{\Sigma}_{ii} = \sigma^{2}_{i} \]

2. Da alle Diagonaleinträge nichtnegativ sind, ist die Spur (trace) (die Summe der Diagonaleinträge) dieser Kovarianzmatrix nichtnegativ:

\[ tr(\boldsymbol{\Sigma}) = \sum^{n}_{i=1}\boldsymbol{\Sigma}_{ii} \geq 0 \]

3. Da \( \boldsymbol{\Sigma}_{ij} = \sigma_{ij} = \sigma_{ji} = \boldsymbol{\Sigma}_{ji} \), ist die Kovarianzmatrix symmetrisch:

\[ \boldsymbol{\Sigma} = \boldsymbol{\Sigma}^{T} \]

4. Die Kovarianzmatrix ist positiv semidefinit (positive semidefinite).
   Die Matrix \( \boldsymbol{A} \) heißt positiv semidefinit, wenn für jeden Vektor \( \boldsymbol{v} \neq 0 \) gilt: \( \boldsymbol{v}^{T}\boldsymbol{A}\boldsymbol{v} \geq 0 \).
   Die Eigenwerte von \( \boldsymbol{A} \) sind nichtnegativ.

Kovarianzmatrix und Erwartungswert

Betrachten wir einen Vektor \( \boldsymbol{x} \) mit \( k \) Elementen:

\[ \boldsymbol{x} = \left[ \begin{matrix} x_{1}\\ x_{2}\\ \vdots \\ x_{k}\\ \end{matrix} \right] \]

Die Einträge des Vektors sind Zufallsvariablen mit endlicher Varianz. Die Kovarianzmatrix des Vektors \( \boldsymbol{x} \) ist gegeben durch:

Der Beweis:

\[ COV(\boldsymbol{x}) = E \left( \left[ \begin{matrix} (x_{1} - \mu_{x_{1}})^{2} & (x_{1} - \mu_{x_{1}})(x_{2} - \mu_{x_{2}}) & \cdots & (x_{1} - \mu_{x_{1}})(x_{k} - \mu_{x_{k}}) \\ (x_{2} - \mu_{x_{2}})(x_{1} - \mu_{x_{1}}) & (x_{2} - \mu_{x_{2}})^{2} & \cdots & (x_{2} - \mu_{x_{2}})(x_{k} - \mu_{x_{k}}) \\ \vdots & \vdots & \ddots & \vdots \\ (x_{k} - \mu_{x_{k}})(x_{1} - \mu_{x_{1}}) & (x_{k} - \mu_{x_{k}})(x_{2} - \mu_{x_{2}}) & \cdots & (x_{k} - \mu_{x_{k}})^{2} \\ \end{matrix} \right] \right) = \]

\[ = E \left( \left[ \begin{matrix} (x_{1} - \mu_{x_{1}}) \\ (x_{2} - \mu_{x_{2}}) \\ \vdots \\ (x_{k} - \mu_{x_{k}}) \\ \end{matrix} \right] \left[ \begin{matrix} (x_{1} - \mu_{x_{1}}) & (x_{2} - \mu_{x_{2}}) & \cdots & (x_{k} - \mu_{x_{k}}) \end{matrix} \right] \right) = \]

\[ = E \left( \left( \boldsymbol{x} - \boldsymbol{\mu}_{x} \right) \left( \boldsymbol{x} - \boldsymbol{\mu}_{x} \right)^{T} \right) \]

Multivariate Normalverteilung

Wir kennen bereits die univariate Normalverteilung. Sie wird durch eine glockenförmige Gauß-Funktion beschrieben:

\[ p(x|\mu,\sigma) = \frac{1}{\sqrt{2\pi\sigma^{2}}} exp \left( -\frac{(x-\mu)^{2}}{2\sigma^{2}} \right)\]

Die Normalverteilung wird geschrieben als:

\[ \mathcal{N}(\mu,\sigma^{2})\]

Die multivariate Normalverteilung ist eine Verallgemeinerung der univariaten Normalverteilung für eine mehrdimensionale Zufallsvariable.

Die \( n \)-dimensionale multivariate Normalverteilung wird beschrieben durch:

\[ p(\boldsymbol{x}|\boldsymbol{\mu},\boldsymbol{\Sigma}) = \frac{1}{\sqrt{(2\pi)^{n}|\boldsymbol{\Sigma}|}} exp \left( -\frac{1}{2}(\boldsymbol{x} - \boldsymbol{\mu})^{T}\boldsymbol{\Sigma}^{-1}(\boldsymbol{x} - \boldsymbol{\mu}) \right)\]

Wobei:

• \( \boldsymbol{x} \) ist ein \( n \)-dimensionaler Zufallsvektor
• \( \boldsymbol{\mu} \) ist ein \( n \)-dimensionaler Mittelwertvektor
• \( \boldsymbol{\Sigma} \) ist eine quadratische \( n \times n \)-Kovarianzmatrix

Bivariate Normalverteilung

Eine bivariate (zweidimensionale) Normalverteilung umfasst zwei normalverteilte Zufallsvariablen. Wir konzentrieren uns auf die bivariate Normalverteilung, da sie die höchste Dimension der multivariaten Normalverteilung ist, die wir noch anschaulich visualisieren können.

Die folgende Grafik zeigt die bivariate Gauß-Funktion.

Konfidenzintervalle

Ein Konfidenzintervall (confidence interval) gibt für die univariate Normalverteilung an, mit welcher Wahrscheinlichkeit ein Parameter zwischen zwei Werten um den Mittelwert liegt.

Bei der univariaten Normalverteilung entspricht die Fläche zwischen den \( 1\sigma \)-Grenzen und der Gauß-Funktion 68,26% der Gesamtfläche unter der Gauß-Funktion.

Für die univariate Normalverteilung können wir die folgenden Aussagen formulieren:

• Das Konfidenzintervall für die Wahrscheinlichkeit von 68,26% ist \( 1\sigma \).
• Das Konfidenzintervall für die Wahrscheinlichkeit von 95,44% ist \( 2\sigma \).
• Das Konfidenzintervall für die Wahrscheinlichkeit von 99,74% ist \( 3\sigma \).

Wir können auch das Konfidenzintervall zu einer beliebigen vorgegebenen Wahrscheinlichkeit für die univariate Normalverteilung bestimmen. Eine Erklärung finden Sie hier.

Die Wahrscheinlichkeit der bivariaten Normalverteilung entspricht einem Volumen unter der dreidimensionalen Gauß-Funktion.

Beispielsweise beträgt das Volumen der dreidimensionalen Gauß-Funktion, abgeschnitten auf dem \( 1\sigma \)-Niveau, 39,35% des Gesamtvolumens der dreidimensionalen Gauß-Funktion.

Die Projektion dieses dreidimensionalen Ausschnitts ist eine Ellipse.

Kovarianzellipse

Zuerst bestimmen wir die Eigenschaften der Kovarianzellipse. Die Kovarianzellipse ist eine Iso-Kontur der Gauß-Verteilung und erlaubt die Visualisierung eines \( 1\sigma \)-Konfidenzintervalls in zwei Dimensionen. Sie liefert außerdem eine geometrische Interpretation der Kovarianzmatrix.

Jede Ellipse kann durch vier Parameter beschrieben werden:

• Ellipsenmittelpunkt \( \mu_{x}, \mu_{y} \)
• Halbe große Achse \( a \)
• Halbe kleine Achse \( b \)
• Orientierungswinkel \( \theta \)

Der Ellipsenmittelpunkt ist der Mittelwert der Zufallsvariablen:

\[ \mu_{x} = \frac{1}{N}\sum^{N}_{i=1}x_{i} \]

\[ \mu_{y} = \frac{1}{N}\sum^{N}_{i=1}y_{i} \]

Die Längen der Ellipsenachsen sind die Quadratwurzeln der Eigenwerte der Kovarianzmatrix der Zufallsvariablen:

• Die Länge der halben großen Achse \( a \) ergibt sich aus der Quadratwurzel des größten Eigenwerts
• Die Länge der halben kleinen Achse \( b \) ergibt sich aus der Quadratwurzel des zweitgrößten Eigenwerts

Die Orientierung der Ellipse entspricht der Orientierung des Eigenvektors der Kovarianzmatrix, der zum größten Eigenwert gehört.

\[ \theta = arctan \left( \frac{v_{y}}{v_{x}} \right)\]

Wobei:

• \( v_{x} \) ist die \(x\)-Koordinate des Eigenvektors, der zum größten Eigenwert gehört
• \( v_{y} \) ist die \(y\)-Koordinate des Eigenvektors, der zum größten Eigenwert gehört

Sie können ein wissenschaftliches Softwarepaket verwenden, um die Parameter der Kovarianzellipse zu berechnen.

import numpy as np

C = np.array([[5, -2],[-2, 1]]) # define covariance matrix

eigVal, eigVec = np.linalg.eig(C) # find eigenvalues and eigenvectors

a = np.sqrt(eigVal[0]) # half-major axis length
b = np.sqrt(eigVal[1]) # half-minor axis length

# ellipse  orientation  angle
theta = np.arctan(eigVec[1, 0] / eigVec[0, 0])  
												

C = [5 -2; -2 1]; % define covariance matrix

[eigVec, eigVal] = eig(C); % find eigenvalues and eigenvectors


if eigVal(1,1) > eigVal(2,2) % get the highest eigenvalue index

    a = sqrt(eigVal(1,1)); % half-major axis length
    b = sqrt(eigVal(2,2)); % half-minor axis length

    theta = atan(eigVec(2,1) / eigVec(1,1));  % ellipse angle (radians)
else

    a = sqrt(eigVal(2,2)); % half-major axis length
    b = sqrt(eigVal(1,1)); % half-minor axis length

    theta = atan(eigVec(2,2) / eigVec(2,1));  % ellipse angle (radians)
end
												

Konfidenzellipse

In vielen Anwendungen ist man daran interessiert, die Grenzen für eine bestimmte Wahrscheinlichkeit zu finden. Für eine Wahrscheinlichkeit von 95% suchen wir beispielsweise die Grenze, die 95% des Volumens der Gauß-Funktion einschließt.

Die Projektion dieser Grenze auf die \( x\text{-}y \)-Ebene ist die Konfidenzellipse (confidence ellipse).

Wir möchten einen elliptischen Skalierungsfaktor (elliptical scale factor) \( k \) finden, der die Kovarianzellipse zur Konfidenzellipse erweitert, die einer Wahrscheinlichkeit von 95% entspricht.

Da \( \sigma_{x} \) und \( \sigma_{y} \) die Standardabweichungen stochastisch unabhängiger Zufallsvariablen darstellen, kann das Additions-Theorem der Chi-Quadrat-Verteilung verwendet werden, um zu zeigen, dass die zu einer Konfidenzellipse gehörige Wahrscheinlichkeit gegeben ist durch:

\[ p = 1 - exp \left( -\frac{1}{2}k^{2} \right) \]

Für eine Kovarianzellipse gilt \( k = 1 \). Damit ist die zu einer Kovarianzellipse gehörige Wahrscheinlichkeit:

\[ p = 1 - exp \left( -\frac{1}{2} \right) = 39.35\% \]

Für eine gegebene Wahrscheinlichkeit können wir den elliptischen Skalierungsfaktor bestimmen:

\[ k = \sqrt{-2ln(1-p)} \]

Für die Wahrscheinlichkeit von 95%:

\[ k = \sqrt{-2ln(1-0.95)} = 2.45\]

Die Eigenschaften der Konfidenzellipse, die einer Wahrscheinlichkeit von 95% entspricht, sind:

• Der Ellipsenmittelpunkt \( (\mu_{x}, \mu_{y}) \) ist identisch zur Kovarianzellipse
• Der Orientierungswinkel \( \theta \) ist identisch zur Kovarianzellipse
• Die Länge der halben großen Achse ist \( 2{,}45a \) – eine skalierte halbe große Achse der Kovarianzellipse
• Die Länge der halben kleinen Achse ist \( 2{,}45b \) – eine skalierte halbe kleine Achse der Kovarianzellipse