Wertekontinuierliche lineare Systeme

Kalman Filter

Prädiktion

Filterung

Kalman Filter (vektoriell) herleiten

Prädiktion

Systemabbildung

Schritte

1. 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 (+)$$

2. 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}$$

   1. 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}$$

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

Schritte

1. 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}$$

2. 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}$$

3. 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.

   1. 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}$$
   2. $\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}$$

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

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