Kovarianzaktualisierungsgleichung (Covariance Update Equation)
Die Kovarianzaktualisierungsgleichung lautet:
\[ \boldsymbol{P}_{n,n} = \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \boldsymbol{P}_{n,n-1} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \boldsymbol{K}_{n}\boldsymbol{R}_{n}\boldsymbol{K}_{n}^{T} \]
Wobei:
| \( \boldsymbol{P}_{n,n} \) | die Kovarianzmatrix der aktuellen Zustandsschätzung ist |
| \( \boldsymbol{P}_{n,n-1} \) | die a-priori-Schätzkovarianz (prior estimate covariance) des aktuellen Zustands ist (aus dem vorherigen Zeitschritt vorhergesagt) |
| \( \boldsymbol{K}_{n} \) | der Kalman-Gain (Kalman Gain) ist |
| \( \boldsymbol{H} \) | die Beobachtungsmatrix (observation matrix) ist |
| \( \boldsymbol{R}_{n} \) | die Messrausch-Kovarianzmatrix (measurement noise covariance matrix) ist |
| \( \boldsymbol{I} \) | eine Einheitsmatrix (Identity Matrix) ist (die quadratische \( n \times n \)-Matrix mit Einsen auf der Hauptdiagonalen und Nullen sonst) |
Herleitung der Kovarianzaktualisierungsgleichung (Covariance Update Equation Derivation)
Dieser Abschnitt enthält die Herleitung der Kovarianzaktualisierungsgleichung. Manche werden das vielleicht zu detailliert finden, aber anderen hilft es, die Gleichung besser zu verstehen.
Sie können zum nächsten Thema springen, wenn Sie sich nicht für die Herleitung interessieren.
Für die Herleitung verwende ich die folgenden vier Gleichungen:
| Gl.-Nr. | Gleichung | Hinweise |
|---|---|---|
| 1 | \( \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} ( \boldsymbol{z}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} ) \) | Zustandsaktualisierungsgleichung (State Update Equation) |
| 2 | \( \boldsymbol{z}_{n} = \boldsymbol{Hx}_{n} + \boldsymbol{v}_{n} \) | Messgleichung (Measurement Equation) |
| 3 | \( \boldsymbol{P}_{n,n} = E\left( \boldsymbol{e}_{n}\boldsymbol{e}_{n}^{T} \right) = E\left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right)^{T} \right) \) | Schätzkovarianz (Estimate Covariance) |
| 4 | \( \boldsymbol{R}_{n} = E\left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T} \right) \) | Messkovarianz (Measurement Covariance) |
Wir leiten die aktuelle Schätzkovarianz (\( \boldsymbol{P}_{n,n} \)) als Funktion des Kalman-Gains (Kalman Gain) \( \boldsymbol{K}_{n} \) her.
| Hinweise | |
|---|---|
| \( \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} ( \boldsymbol{z}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} ) \) | Zustandsaktualisierungsgleichung |
| \( \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} ( \boldsymbol{Hx}_{n} + \boldsymbol{v}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} ) \) | Setze die Messgleichung in die Zustandsaktualisierungsgleichung ein |
| \( \boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \) | Schätzfehler (estimate error) |
| \( \boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} - \boldsymbol{K}_{n} \left(\boldsymbol{Hx}_{n} + \boldsymbol{v}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} \right) \) | Setze \( \boldsymbol{\hat{x}}_{n,n} \) ein |
| \( \boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} - \boldsymbol{K}_{n}\boldsymbol{Hx}_{n} - \boldsymbol{K}_{n}\boldsymbol{v}_{n} + \boldsymbol{K}_{n}\boldsymbol{H \hat{x}}_{n,n-1} \) | Ausmultiplizieren |
| \( \boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} - \boldsymbol{K}_{n}\boldsymbol{H}\left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \) | Klammer \( (\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1}) \) aus |
| \( \boldsymbol{e}_{n} = \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n}\) | |
| \( \boldsymbol{P}_{n,n} = E\left( \boldsymbol{e}_{n}\boldsymbol{e}_{n}^{T} \right) = E\left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right)^{T} \right) \) | Schätzkovarianz |
| \( \boldsymbol{P}_{n,n} = E\left( \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right) \\ \times \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right) \) | Setze \( \boldsymbol{e}_{n} \) ein |
| \( \boldsymbol{P}_{n,n} = E\left( \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right) \\ \times \left( \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \right)^{T} - \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n}\right) ^{T} \right) \right) \) | Ausmultiplizieren |
| \( \boldsymbol{P}_{n,n} = E\left(\left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right) \\ \times \left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} - \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n}\right)^{T} \right) \right) \) | Verwende die Transponierungseigenschaft: \( (\boldsymbol{AB})^{T} = \boldsymbol{B}^{T}\boldsymbol{A}^{T} \) |
| \( \boldsymbol{P}_{n,n} = E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \\ - \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \\ - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \\ + \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right) \) | Ausmultiplizieren |
| \( \boldsymbol{P}_{n,n} = E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right) \\ - \color{red}{E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right)} \\ - \color{red}{E \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right)} \\ + E \left( \color{blue}{\boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} }\right) \) | Verwende die Regel \( E(X \pm Y) = E(X) \pm E(Y) \) |
|
\( \color{red}{E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right) = 0} \) \( \color{red}{E \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right) = 0} \) |
Da \( (\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1}) \) und \( \boldsymbol{v}_{n} \) unabhängig sind, sind die Kreuzterme (cross terms) im Ausdruck innerhalb des Erwartungswerts gleich Null. |
| \( \boldsymbol{P}_{n,n} = E \left( \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right) \\ + E \left( \color{blue}{\boldsymbol{K}_{n}\boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T}\boldsymbol{K}_{n}^{T}}\right) \) | Verwende die Transponierungseigenschaft: \( (\boldsymbol{AB})^{T} = \boldsymbol{B}^{T}\boldsymbol{A}^{T} \) |
| \( \boldsymbol{P}_{n,n} = \boldsymbol{ \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)} \color{green}{E \left( \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \right)} \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \\ + \boldsymbol{K}_{n} \color{blue}{ E \left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T} \right) } \boldsymbol{ K}_{n}^{T} \) | Verwende die Regel \( E(aX) = aE(X) \) |
|
\( \color{green}{E \left( \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \right) = \boldsymbol{P}_{n,n-1}} \) \( \color{blue}{ E \left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T}\right) = \boldsymbol{R}_{n}} \) |
\( \color{green}{\boldsymbol{P}_{n,n-1}} \) ist die a-priori-Schätzkovarianz (prior estimate covariance) \( \color{blue}{\boldsymbol{R}_{n}} \) ist die Messkovarianz (measurement covariance) |
| \( \boldsymbol{P}_{n,n} = \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \color{green}{\boldsymbol{P}_{n,n-1} } \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \boldsymbol{K}_{n} \color{blue}{ \boldsymbol{R}_{n}} \boldsymbol{K}_{n}^{T} \) | Kovarianzaktualisierungsgleichung! |