Wertekontinuierliche lineare Systeme
Kalman Filter
Prädiktion
$$ \underline{\hat{x}}_k^p = \mathbf{A}_{k-1}\underline{\hat{x}}_{k-1}^e + \mathbf{B}_{k-1} \underline{\hat{u}}_{k-1} $$ $$ \mathbf{C}_k^p = \mathbf{A}_{k-1} \mathbf{C}_{k-1}^e A_{k-1}^\top + \mathbf{B}_{k-1} \mathbf{C}_{k-1}^w \mathbf{B}_{k-1}^\top $$Filterung
$$ \mathbf{K}_k = \mathbf{C}_k^p \mathbf{H}_k^\top (\mathbf{C}_k^v + \mathbf{H}_k \mathbf{C}_k^p \mathbf{H}_k ^\top)^{-1} \tag{Kalman Gain} $$ $$ \underline{\hat{x}}_k^e = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \underline{\hat{x}}_k^p + \mathbf{K}_k \underline{\hat{y}}_k = \underline{\hat{x}}_k^p + \mathbf{K}_k(\underline{\hat{y}}_k - \mathbf{H}_k \underline{\hat{x}}_k^p) $$ $$ \mathbf{C}_k^e = (\mathbf{I} - \mathbf{K}_k\mathbf{H}_k)\mathbf{C}_k^p = \mathbf{C}_k^p - \mathbf{C}_k^p \mathbf{H}_k^\top (\mathbf{C}_k^v + \mathbf{H}_k \mathbf{C}_k^p \mathbf{H}_k ^\top)^{-1}\mathbf{H}_k \mathbf{C}_k^p $$Kalman Filter (vektoriell) herleiten
Prädiktion
Systemabbildung
$$ \underline{x}_{k+1}=\mathbf{A}_{k} \cdot \underline{x}_{k}+\mathbf{B}_{k} \cdot \underbrace{\left(\underline{\tilde{u}}_{k}+\underline{w}_{k}\right)}_{\underline{u_k}} $$Schritte
Berechnung des Erwartungswerts für $k+1$
$$ E\left\{\underline{x}_{k+1}\right\}=\mathbf{A}_{k} \cdot \underline{\hat{x}}_{k|1: m}+\mathbf{B}_{k} \tilde{\underline{u}}_{k} \qquad (+) $$Berechnung der Kovarianzmatrix $C_{k+1|1:m}^x$ mit der Annahme, dass Zustand und Systemrauschen unkorreliert sind
$$ \begin{aligned} \underline{x}_{k+1} &=\mathbf{A}_{k} \underline{x}_{k}+\mathbf{B}_{k} \underline{u}_{k} \\ &=\left[\begin{array}{ll} \mathbf{A}_{k} & \mathbf{B}_{k} \end{array}\right]\left[\begin{array}{c} \underline{x}_{k} \\ \underline{u}_{k} \end{array}\right] \end{aligned} $$Berechne $\operatorname{Cov}\left\{\left[\begin{array}{c} \underline{x}_{k} \\ \underline{\tilde{u}}_{k} \end{array}\right]\right\}$
$$ \begin{aligned} \underline{x}_{k+1}-\hat{\underline{x}}_{k+1} &=\left[\begin{array}{ll} \mathbf{A}_{k} & \mathbf{B}_{k} \end{array}\right]\left[\begin{array}{c} \underline{x}_{k}-\hat{\underline{x}}_{k} \\ \underline{u}_{k}-\underline{\hat{u}}_{k} \end{array}\right] \\ &=\left[\begin{array}{ll} \mathbf{A}_{k} & \mathbf{B}_{k} \end{array}\right]\left[\begin{array}{c} \underline{x}_{k}-\underline{\hat{x}}_{k} \\ \underline{w}_{k} \end{array}\right] \end{aligned} $$ $$ \begin{aligned} \operatorname{Cov}\left\{\left[\begin{array}{c} \underline{x}_{k} \\ \underline{\tilde{u}}_{k} \end{array}\right]\right\} &=E\left\{\left[\begin{array}{c} \underline{x}_{k}-\underline{\hat{x}}_{k} \\ \underline{w}_{k} \end{array}\right]\left[\left(\underline{x}_{k}-\underline{\hat{x}}_{k}\right)^{\top} \underline{w}_{k}^{\top}\right]\right\} \\ &=\left[\begin{array}{cc} C_{k \mid 1: m}^{x} & 0 \\ 0 & C_{k}^{w} \end{array}\right] \end{aligned} $$$\operatorname{Cov}\left\{\left[\begin{array}{c} \underline{x}_{k} \\ \underline{\tilde{u}}_{k} \end{array}\right]\right\}$ in Berechnung von $C_{k+1|1:m}^x$ einsetzen
$$ \begin{aligned} \mathbf{C}_{k+1 \mid 1 : m}^{x} &=E\left\{\left(\underline{x}_{k+1}-\hat{x}_{k+1}\right)\left(x_{k+1} - \hat{\underline{x}}_{k+1}\right)^\top\right\} \\ &=\left[\begin{array}{ll} \mathbf{A}_{k} & \mathbf{B}_{k} \end{array}\right] \cdot E\left\{\left[\begin{array}{c} \underline{x}_{k}-\hat{\underline{x}}_{k} \\ \underline{w}_{k} \end{array}\right]\left[\begin{array}{ll} \underline{x}_{k}-\hat{\underline{x}}_{k} & \underline{w}_{k} \end{array}\right]^\top\right\} \cdot\left[\begin{array}{l} \mathbf{A}_{k}^{\top} \\ \mathbf{B}_{k}^{\top} \end{array}\right] \\\\ &=\left[\begin{array}{ll} \mathbf{A}_{k} & \mathbf{B}_{k} \end{array}\right] \cdot\left[\begin{array}{cc} \mathbf{C}_{k \mid 1:m} & 0 \\ 0 & \mathbf{C}_{k}^{w} \end{array}\right] \cdot\left[\begin{array}{l} \mathbf{A}_{k}^{\top} \\ \mathbf{B}_{k}^{\top} \end{array}\right] \\ &=\mathbf{A}_{k} \cdot \mathbf{C}_{k \mid 1: m}^{x} \mathbf{A}_{k}^{\top}+\mathbf{B}_{k} \mathbf{C}_{k}^{w} \mathbf{B}_{k}^{\top} \qquad(++) \end{aligned} $$
Filterung
Messabbildung
$$ \underline{y}_{k}=\mathbf{H}_{k} \cdot \underline{x}_{k}+\underline{v}_{k} $$Schritte
Schreibe $\underline{x}_k^e$ als lineare Kombination von $\underline{x}_k^p$ und $\underline{y}_k$
$$ \underline{x}_{k}^e=\mathbf{K}_{k}^{(1)} \underline{x}_{k}^p+\mathbf{K}_{k}^{(2)} \underline{y}_{k} $$Aus BLUE Filter ergibt sich
$$ E\{\underline{x}_{k}^e\}=E\{\mathbf{K}_{k}^{(1)} \underline{x}_{k}^p+\mathbf{K}_{k}^{(2)} \underline{y}_{k}\} $$$\Rightarrow$
$$ \begin{aligned} \mathbf{K}_{k}^{(1)} &= \mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k} \\ \mathbf{K}_{k}^{(2)} &= \mathbf{K}_{k} \end{aligned} $$und
$$ \underline{x}_{k}^e=(\mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k}) \underline{x}_{k}^p+\mathbf{K}_{k} \underline{y}_{k} $$Berechne Kovarianzmatrix $\mathbf{C}_k^e$
$$ \mathbf{C}_{k}^{e}\left(\mathbf{K}_{k}\right)=\operatorname{Cov}\{\underline{x}_k^e - \underline{x}\} = \left(\mathbf{I}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \mathbf{C}_{k}^{p}\left(\mathbf{I}-\mathbf{K}_{k} \mathbf{H}_{k}\right)^{\top}+\mathbf{K}_{k} C_{k}^{v} \mathbf{K}_{k}^{\top} $$Wir suche $\mathbf{K}_{k}$ so, dass der resultierende Schätzer MINIMAL kovarianz aufweist.
Auf skalares Gütemaß zurückzuführen
$$ P(\mathbf{K}_{k}) = \underline{e}^\top \left( \left(\mathbf{I}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \mathbf{C}_{k}^{p}\left(\mathbf{I}-\mathbf{K}_{k} \mathbf{H}_{k}\right)^{\top}+\mathbf{K}_{k} C_{k}^{v} \mathbf{K}_{k}^{\top}\right) \underline{e} $$- $\frac{\partial}{\partial \mathbf{K}_{k}} P(\mathbf{K}_{k})\overset{!}{=} 0 \Rightarrow$ $$ \mathbf{K}_k = \mathbf{C}_k^p \mathbf{H}_k^\top (\mathbf{C}_k^v + \mathbf{H}_k \mathbf{C}_k^p \mathbf{H}_k^\top)^{-1} $$
$\mathbf{K}_k$ in $\underline{x}_{k}^e$ und $\mathbf{C}_{k}^{e}$ einsetzen
Ergebnis von “Gauß mal Gauß”
Drei Gütemaße für die „Größe“ einer Kovarianzmatrix
Mögliche Gütemaße für generelles Vergleichen von Kovarianzmatrizen:
$$ f: \mathbb{R}^{n \times n} \to \mathbb{R}^1 $$Funktion, die einer Kovarianzmatrix einen Skalar zuordnen kann, denn man kann nur Skalare direkt miteinander vergleichen.
Drei Gütemaße
- Projektion mit Einheitsvektor
- Spur
- Determinante
