Docs
Notes
SI
Zusammenfassung
Schwach nichtlineare wertekontinuierliche Systeme

Schwach nichtlineare wertekontinuierliche Systeme

Lineare Vs. Nichtlineare Systeme

LinearNichtlinear
Systemabbildung$\underline{x}_{k+1} = \mathbf{A}_k \underline{x}_k + \mathbf{B}_k (\underline{u}_k + \underline{w}_k)$$\underline{x}_{k+1} = \underline{a}_k(\underline{x}_k, \underline{u}_k, \underline{w}_k)$
Messabbildung$\underline{y}_{k} = \mathbf{H}_k \underline{x}_k + \underline{v}_k$$\underline{y}_k = \underline{h}_k (\underline{x}_k, \underline{v}_k)$

Extended Kalman Filter (EKF)

💡 Idee: Linearisierung mit Tylorentwicklung 1. Ordnung um die beste verfügbare Schätzung, um den (linear) Kalman-Filter zu vewenden.

  • Systemabbildung

    $$ \underline{a}_{k}\left(\underline{x}_{k}, \underline{u}_{k}\right) \approx \underbrace{\underline{a}_{k}\left(\underline{\overline{x}}_k, \underline{\overline{u}}_k\right)}_{\text{Nomialteil}}+\underbrace{\mathbf{A}_{k}\left(\underline{x}_k-\underline{\overline{x}}_k\right)+\mathbf{B}_{k}\left(\underline{u}_{k}-\underline{\overline{u}}_k\right)}_{\text{Differentialteil}} $$

  • Messabbildung

    $$ \underline{h}_{k}\left(\underline{x}_{k}, \underline{v}_{k}\right) \approx \underbrace{\underline{h}_{k}\left(\underline{\bar{x}}_{k}, \underline{\bar{v}}_{k}\right)}_{\text{Nomialteil}}+ \underbrace{\mathbf{H}_{k} \cdot \left(\underline{x}_{k}-\underline{\bar{x}}_{k}\right)+\mathbf{L}_{k} \cdot\left(\underline{v}_{k}-\underline{\bar{v}}_{k}\right)}_{\text{Differentialteil}} $$
Üb 7, A2

Prädiktion

  • Berechnung Erwartungswert über nichtlineare Funktion

    $$ \underline{\hat{x}}_{k+1}^{p}=\underline{a}_{k}\left(\underline{\hat{x}}_{k}^{e}, \hat{\underline{u}}_{k}\right) $$

  • Berechnung Kovarianzmatrix über die Linearisierung

    $$ \mathbf{C}_{k+1}^{p} \approx \mathbf{A}_{k} \mathbf{C}_{k}^{e} \mathbf{A}_{k}^{\top}+\mathbf{C}_{k}^{w^{\prime}}=\mathbf{A}_{k} \mathbf{C}_{k}^{e} \mathbf{A}_{k}^{\top}+\mathbf{B}_{k} \mathbf{C}_{k}^{w} \mathbf{B}_{k}^{\top} $$

    mit

    $$ \mathbf{A}_k = \left.\frac{\partial \underline{a}_{k}\left(\underline{x}_{k}, \underline{u}_{k}\right)}{\partial \underline{x}_{k}^{\top}}\right|_{\underline{x}_{k}=\underline{\hat{x}}_{k-1}^{e}, \underline{u}_{k}=\hat{\underline{u}}_{k}} \qquad \mathbf{B}_k = \left.\frac{\partial \underline{a}_{k}\left(\underline{x}_{k}, \underline{u}_{k}\right)}{\partial \underline{u}_{k}^{\top}}\right|_{\underline{x}_{k}=\underline{\hat{x}}_{k-1}^{e}, \underline{u}_{k}=\hat{\underline{u}}_{k}} $$

Filterung

  • Linearisierung um $\underline{x}_k$ und $\underline{v}_k$

    $$ \mathbf{H}_{k}=\left.\frac{\partial \underline{h}_{k}\left(\underline{x}_{k}, \underline{v}_{k}\right)}{\partial \underline{x}_{k}^{\top}}\right|_{\underline{x}_{k}=\underline{\hat{x}}_{k}^{p}, \underline{v}_{k}=\underline{\hat{v}}_{k}} \qquad \mathbf{L}_{k}=\left.\frac{\partial \underline{h}_{k}\left(\underline{x}_{k}, \underline{v}_{k}\right)}{\partial \underline{v}_{k}^{\top}}\right|_{\underline{x}_{k}=\underline{\hat{x}}_{k}^{p}, \underline{v}_{k}=\underline{\hat{v}}_{k}} $$

  • KF Filterung schriit mit Linearisierung

    $$ \begin{aligned} \mathbf{K}_{k}&=\mathbf{C}_{k}^{p} \mathbf{H}_{k}^{\top}\left(\mathbf{L}_{k} \mathbf{C}_{k}^{v} \mathbf{L}_{k}^{\top}+\mathbf{H}_{k} \mathbf{C}_{k}^{p} \mathbf{H}_{k}^{T}\right)^{-1} \\\\ \hat{\underline{x}}_{k}^{e}&=\hat{\underline{x}}_{k}^{p}+\mathbf{K}_{k}\left[\hat{\underline{y}}_{k}-\underline{h}_{k}\left(\hat{\underline{x}}_{k}^{p}, \hat{\underline{v}}_{k}\right)\right] \overset{\underline{v} \text{ mittelwertfrei}}{=} \hat{\underline{x}}_{k}^{p}+\mathbf{K}_{k}\left[\hat{\underline{y}}_{k}-\underline{h}_{k}\left(\hat{\underline{x}}_{k}^{p}, 0\right)\right]\\\\ \mathbf{C}_{k}^{e}&=\mathbf{C}_{k}^{p}-\mathbf{K}_{k} \mathbf{H}_{k} \mathbf{C}_{k}^{p} = (\mathbf{I} - \mathbf{K}_{k} \mathbf{H}_{k})\mathbf{C}_{k}^{p} \end{aligned} $$

(Linear) KF vs. EKF

(Linear) KFEKF
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$		$\underline{\hat{x}}_{k+1}^{p}=\underline{a}_{k}\left(\underline{\hat{x}}_{k}^{e}, \hat{\underline{u}}_{k}\right)$
$\mathbf{C}_{k+1}^{p} \approx \mathbf{A}_{k} \mathbf{C}_{k}^{e} \mathbf{A}_{k}^{\top}+\mathbf{C}_{k}^{w^{\prime}}=\mathbf{A}_{k} \mathbf{C}_{k}^{e} \mathbf{A}_{k}^{\top}+\mathbf{B}_{k} \mathbf{C}_{k}^{w} \mathbf{B}_{k}^{\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}$
$\underline{\hat{x}}_k^e = \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$		$\begin{aligned}
\mathbf{K}_{k}&=\mathbf{C}_{k}^{p} \mathbf{H}_{k}^{\top}\left(\mathbf{L}_{k} \mathbf{C}_{k}^{v} \mathbf{L}_{k}^{\top}+\mathbf{H}_{k} \mathbf{C}_{k}^{p} \mathbf{H}_{k}^{T}\right)^{-1} \\
\hat{\underline{x}}_{k}^{e}&=\hat{\underline{x}}_{k}^{p}+\mathbf{K}_{k}\left[\hat{\underline{y}}_{k}-\underline{h}_{k}\left(\hat{\underline{x}}_{k}^{p}, \hat{\underline{v}}_{k}\right)\right] \\
\mathbf{C}_{k}^{e}&=\mathbf{C}_{k}^{p}-\mathbf{K}_{k} \mathbf{H}_{k} \mathbf{C}_{k}^{p} = (\mathbf{I} - \mathbf{K}_{k} \mathbf{H}_{k})\mathbf{C}_{k}^{p}
\end{aligned}$
Auxiliary$\mathbf{A}_k = \left.\frac{\partial \underline{a}_{k}\left(\underline{x}_{k}, \underline{u}_{k}\right)}{\partial \underline{x}_{k}^{\top}}\right|_{\underline{x}_{k}=\underline{\hat{x}}_{k-1}^{e}, \underline{u}_{k}=\hat{\underline{u}}_{k}}$
$\mathbf{B}_k = \left.\frac{\partial \underline{a}_{k}\left(\underline{x}_{k}, \underline{u}_{k}\right)}{\partial \underline{u}_{k}^{\top}}\right|_{\underline{x}_{k}=\underline{\hat{x}}_{k-1}^{e}, \underline{u}_{k}=\hat{\underline{u}}_{k}}$
$\mathbf{H}_{k}=\left.\frac{\partial \underline{h}_{k}\left(\underline{x}_{k}, \underline{v}_{k}\right)}{\partial \underline{x}_{k}^{\top}}\right|_{\underline{x}_{k}=\underline{\hat{x}}_{k}^{p}, \underline{v}_{k}=\underline{\hat{v}}_{k}}$
$\mathbf{L}_{k}=\left.\frac{\partial \underline{h}_{k}\left(\underline{x}_{k}, \underline{v}_{k}\right)}{\partial \underline{v}_{k}^{\top}}\right|_{\underline{x}_{k}=\underline{\hat{x}}_{k}^{p}, \underline{v}_{k}=\underline{\hat{v}}_{k}}$

Probleme

  • Berechnung der posteriore Verteilung nur gut für “schwache” Nichtlinearität

  • Linearisierung nur um einen Punkt

  • Linearisiertes System ist i.A. zeitvariant, auch wenn originalsytstem zeitinvariant ist, da Linearisierung vom Schätzwert abhängt.

Kalman Filter in probabilistischer Form

Filterung

(Annahme: $\underline{x}_k$ und $\underline{y}_k$ sind gemeinsam Gaußverteilt)

  1. Define $\underline{z}:=\left[\begin{array}{l} \underline{x} \\ \underline{y} \end{array}\right]$

  2. Mittelwert und Varianz von $\underline{z}$ berechnen.

    $$ \underline{\mu}_z=\left[\begin{array}{l} \underline{\mu}_x \\ \underline{\mu}_y \end{array}\right]=\frac{1}{L}\sum_{i=1}^L\left[\begin{array}{l} \underline{x}_i \\ \underline{y}_i \end{array}\right], \quad \mathbf{C}_{z} = \frac{1}{L}\sum_{i=1}^L(\underline{z}_i - \underline{\mu}_z)(\underline{z}_i - \underline{\mu}_z)^\top = \left[\begin{array}{ll} \mathbf{C}_{x x} & \mathbf{C}_{x y} \\ \mathbf{C}_{y x} & \mathbf{C}_{y y} \end{array}\right] $$

  3. Filterung in probabilistischer Form mit Messung $\hat{\underline{y}}$

    $$ \begin{aligned} \underline{\hat{x}}_k^e &= \underline{x}_k^p + \mathbf{C}_{xy} \mathbf{C}_{yy}^{-1} (\underline{\hat{y}} - \underline{\mu}_y) \\ \mathbf{C}_k^e &= \mathbf{C}_k^p - \mathbf{C}_{xy} \mathbf{C}_{yy}^{-1} \mathbf{C}_{yx} \end{aligned} $$

Unscented Kalman Filter (UKF)

Üb 7, A3

Unscented Prinzipien

  • Nichtlineare Transformation eines einzelnen Punktes ist einfach
  • Es ist einfach, eine Punktwolke zu finden, deren Stichprobenmittelwert und -varianz mit den Momenten der gegebene Dichte übereinstimmen.
  • Es ist einfach, Mittelwert und Varianz einer Punktwolke zu bestimmen

Bsp: additives Rauschen

$$ \begin{aligned} \underline{x}_{k+1} &= \underline{a}_{k}(\underline{x}_{k}) + \underline{w}_{k} \\ \underline{y}_{k} &= \underline{h}_{k}(\underline{x}_{k}) + \underline{v}_{k} \end{aligned} $$

Prädiktion

  1. Samples/Particles/Punkte propagieren

    $$ \underline{x}_{k}^{p, i} = \underline{a}_{k-1}(\underline{x}_{k-1}^{e, i}) $$

  2. Mittelwert und Varianz basierend auf Samples berechnen

    $$ \begin{aligned} \underline{\hat{x}}_{k}^p &= \frac{1}{L} \sum_{i=1}^L \underline{x}_{k}^{p, i} \\ \mathbf{C}_k^p &= \frac{1}{L} \sum_{i=1}^L (\underline{x}_{k}^{p, i} - \underline{\hat{x}}_{k}^p) (\underline{x}_{k}^{p, i} - \underline{\hat{x}}_{k}^p)^\top + \mathbf{C}_k^w \end{aligned} $$

Fitlerung

  1. Sampling:

    • Für prioren Schätzwert: $2N$ btw. $2N+1$ Samples auf Hauptachsen für Dimension $N$

      Bsp: Im skalaren Fall ($N=1$), 2 Samples:

      $$ > x_1 = \mu_p + \sigma_p \quad x_2 = \mu_p - \sigma_p > $$

    • Ähnlich für Samples vom Mess-Rauschen

      Bsp: Im skalaren Fall ($N=1$), 2 Samples:

      $$ > v_1 = \mu_v + \sigma_v \quad v_2 = \mu_v - \sigma_v > $$

  2. Punkte Propagation

    $$ \underline{y}_{k}^{p, i} = \underline{h}_{k}(\underline{x}_{k}^{p, i}) $$

    bzw.

    $$ \underline{y}_{k}^{i, j} = \underline{h}_{k}(\underline{x}_{k}^{p, i}, \underline{v}_k^j) $$

  3. Verbundraum $\underline{z}=\left[\begin{array}{l} \underline{x} \\ \underline{y} \end{array}\right]$ erstellen (Annahme: $\underline{x}_k$ und $\underline{y}_k$ sind gemeinsam Gaußverteilt). Mittelwert und Varianz von $\underline{z}$ berechnen.

    $$ \underline{\mu}_z=\left[\begin{array}{l} \underline{\mu}_x \\ \underline{\mu}_y \end{array}\right]=\frac{1}{L}\sum_{i=1}^L\left[\begin{array}{l} \underline{x}_i \\ \underline{y}_i \end{array}\right], \quad \mathbf{C}_{z} = \frac{1}{L}\sum_{i=1}^L(\underline{z}_i - \underline{\mu}_z)(\underline{z}_i - \underline{\mu}_z)^\top = \left[\begin{array}{ll} \mathbf{C}_{x x} & \mathbf{C}_{x y} \\ \mathbf{C}_{y x} & \mathbf{C}_{y y} \end{array}\right] $$

  4. Filterung in probabilistischer Form mit Messung $\hat{\underline{y}}$

    $$ \begin{aligned} \underline{\hat{x}}_k^e &= \underline{x}_k^p + \mathbf{C}_{xy} \mathbf{C}_{yy}^{-1} (\underline{\hat{y}} - \underline{\mu}_y) \\ \mathbf{C}_k^e &= \mathbf{C}_k^p - \mathbf{C}_{xy} \mathbf{C}_{yy}^{-1} \mathbf{C}_{yx} \end{aligned} $$

Sampling

Samples nur auf Hauptachsen: Insgesamt $2N$ btw. $2N+1$ ($N$: #Dimensionen)

Vorteil von UKF gegen EKF

  • UKF reduziert möglicherweise den Linearisierungsfehler des EKF
  • Man braucht die Jacobi-Matrizen nicht zu berechnen 👏

Analytische Momente

Üb 7, A4

  1. Verbundraum $\underline{z}$ erstellen

    $$ z := \left[\begin{array}{l} x \\ y \end{array}\right] $$

  2. Mittelwert von $\underline{z}$ berechnen (mithilfe von höheren Momente der Gaußdichte)

    $$ E\{\underline{z}\}=\left[\begin{array}{c} \hat{x}_{p} \\ E\{h(x)\} \end{array}\right] $$

  3. Differenz zwichen $h(x)$ und $E\\{h(x)\\}$ berechnen

    $$ \bar{h}(x)=h(x)-E\{h(x)\} $$

  4. $\operatorname{Cov}\{\underline{z}\}$ berechnen

    $$ \operatorname{Cov}\{\underline{z}\}=\left[\begin{array}{ll} \mathbf{C}_{x x} & \mathbf{C}_{x y} \\ \mathbf{C}_{y x} & \mathbf{C}_{y y} \end{array}\right]=\left[\begin{array}{cc} \sigma_{p}^{2} & E\left\{\left(x-\hat{x}_{p}\right) \bar{h}(x)\right\} \\ E\left\{\left(x-\hat{x}_{p}\right) \bar{h}(x)\right\} & E\left\{\overline{h}^{2}(x)\right\}+\sigma_{v}^{2} \end{array}\right] $$

  5. Filterung in probabilistischer Form.

    $$ \begin{aligned} \underline{\hat{x}}_k^e &= \underline{x}_k^p + \mathbf{C}_{xy} \mathbf{C}_{yy}^{-1} (\underline{\hat{y}} - \underline{\mu}_y) \\ \mathbf{C}_k^e &= \mathbf{C}_k^p - \mathbf{C}_{xy} \mathbf{C}_{yy}^{-1} \mathbf{C}_{yx} \end{aligned} $$

Ensemble Kalman Filter (EnKF)

Üb 6, A4 (f)

💡 Repräsentiere den unsicheren Schätzwert nun per „Streuungsbreite“ einer Punktwolke.

Als „unsicheren Zustand“ verwende $L$ $N$-dim. Vektoren als Samples

$$ \mathcal{X}_{k}=[\underbrace{\underline{x}_{k, 1}}_{\mathbb{R}^N}, \underline{x}_{k, 2}, \ldots, \underline{x}_{k, L}] \in \mathbb{R}^{N \times L}, \quad \mathcal{W}_{k}=\left[\underline{w}_{k, 1}, \underline{w}_{k, 2}, \ldots, \underline{w}_{k, L}\right] \in \mathbb{R}^{N \times L} $$

wobei die Samples als Spalten einer Matrix kompakt aufgefasst werden können.

Prädiktion

  • Nichtlinear

    $$ \mathcal{X}_{k}^p = \underline{a}_{k-1}(\mathcal{X}_{k-1}^e, \underline{u}_{k-1}, \mathcal{W}_{k-1}) $$

  • Linear

    $$ \mathcal{X}_{k}^p = \mathbf{A}_{k-1}\mathcal{X}_{k-1}^e + \mathbf{B}_{k-1}(\underline{u}_{k-1} + \mathcal{W}_{k-1}) $$

Filterung

  • Durchführung der Filterschritt NUR mit Samples
  • Vermeidung der Verwendung der Update-Formeln für Kovarianzmatrix (Reine Representation der Unsicherheiten durch Samples)

Schritte

  1. „Prädizierte“ Mess-Samples berechnen

    • linear

      $$ \mathcal{Y}_k = \mathbf{H}_k \mathcal{X}_{k}^p + \mathcal{V}_{k} $$

    • nichtlinear

      $$ \mathcal{Y}_k = \underline{h}_k (\mathcal{X}_{k}^p, \mathcal{V}_{k}) $$

  2. Kalman Gain berechnen

    $$ \begin{aligned} \mathbf{C}_{x y} &=\frac{1}{L} \sum_{i=1}^{L} \underline{x}_{k, i}^{\mathrm{p}} \cdot \underline{y}_{k, i}^{\top} \\ &=\frac{1}{L} \mathcal{X}_{k}^{\mathrm{p}} \cdot \mathcal{Y}_{k}^{\top} \\\\ \mathbf{C}_{y y} &=\frac{1}{L} \sum_{i=1}^{L} \underline{y}_{k, i} \cdot \underline{y}_{k, i}^{\top} \\ &=\frac{1}{L} \mathcal{Y}_{k} \cdot \mathcal{Y}_{k}^{\top} \\\\ \mathbf{K} &=\mathbf{C}_{x y} \cdot \mathbf{C}_{y y}^{-1} \end{aligned} $$

  3. Filterschritt mit der tatsächlichen Messung $\underline{\hat{y}}_k$

    $$ \mathcal{X}_{k}^e = \mathcal{X}_{k}^p + \mathbf{K} (\underline{\hat{y}}_k \cdot \underline{\mathbb{1}}^\top - \mathcal{Y}_k) $$