Schwach nichtlineare wertekontinuierliche Systeme

Lineare Vs. Nichtlineare Systeme

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

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

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)

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

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

0. 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 >$$

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

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

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

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

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)

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

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

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)$$