Feature-based sensor model
Feature-Based Measurement Models
Einfache Sprache
Das Feature-based sensor model ist ein Wahrscheinlichkeitstheoretische Wahrnehmungsmodelle bei dem in Merkmalgestützte Karte Triangulation genutzt wird. Dazu werden sogenannte Landmarks genutzt und es wird angenommen man hat einen range and bearing sensor.
Herleitung
Gegeben das Problem in Wahrscheinlichkeitstheoretische Wahrnehmungsmodelle, wobei $m$ eine Merkmalgestützte Karte ist, sei $f$ eine Funktion die aus $z_t$ eine Liste von Merkmalen (features) extrahiert, die jeweils aus Entfernung $r$, Peilung $\phi$ und Signatur $s$ bestehen.
$$f(z_t) = \left[f_t^1,f_t^2,\ldots\right] =\left[\left( \begin{array}{c} r_t^1 \\ \phi_t^1 \\ s_t^1\end{array} \right),\left( \begin{array}{c} r_t^2 \\ \phi_t^2 \\ s_t^2\end{array} \right),\ldots\right]$$Es können unterschiedlich viele Merkmale in der Liste sein.
Wir nehmen an das die einzelnen Merkmale unabhängig voneinander sind, also das
$$p(f(z_t)\mid x_t,m) = \prod_i p(f_t^i\mid x_t,m)$$Dadurch können wir die Merkmale einzeln Verarbeiten.
In der Merkmalgestützte Karte $m$ sei für jedes Landmark $m_i$ die Position definiert als $\left(\begin{array}{c}m_{i,x} \\m_{i,y} \end{array} \right)$. Wir nehmen jetzt an das in $t$ der $j$-te Landmark zu dem $i$-te Merkmal gehört (auch Korrespondenz genannt). Dann ist $f_t^i$ wie folgt definiert:
$$\left( \begin{array}{c} r_t^i \\ \phi_t^i \\ s_t^i\end{array} \right) = \left( \begin{array}{c} \sqrt{(m_{j,x}-x)^2+ (m_{j,y}-y)^2} \\ \mathrm{atan2}(m_{j,y}-y, m_{j,x}-x) - \theta \\ s_j\end{array}\right) + \left( \begin{array}{c} \varepsilon_{\sigma_r^2} \\ \varepsilon_{\sigma_\phi^2} \\ \varepsilon_{\sigma_s^2} \end{array} \right)$$Wobei $\varepsilon_x$ normalverteilt um Null mit Standardabweichung $x$ ist.
Bekannte Korrespondenzen
Geschlossene Form
Wir nehmen an das jedes Landmark genau einem Merkmal zugeordnet werden kann. Wir definieren dazu eine Korrespondenzvariable $c_t^i\in\{1,\ldots,N+1\}$ wobei $N$ die Anzahl der Landmarks in $m$ sind. Ist $c_t^i = j\leq N$, dann ist das $i$-te Merkmal das $j$-te Landmark. Falls $c_t^i=N+1$, dann kann das $i$-te Merkmal keinem Landmark aus der $m$ zugeordnet werden.
Daraus ergibt sich folgender Algorithmus um die Wahrscheinkeit $p(f_t^i\mid c_t^i,m,x_t)$ zu berechnen.
\begin{algorithm}
\caption{Merkmalgestützes sensor model}
\begin{algorithmic}
\Input pose $x_t$, map $m$, feature $f_t^i$, correspondance variable $c_t^i$
\Procedure{landmark-model-known-correspondance}{$f_t^i,c_t^i, x_t, m$}
\State $j \gets c_t^i$
\State $\hat r \gets \sqrt{(m_{j,x}-x)^2+ (m_{j,y}-y)^2}$
\state $\hat\phi \gets \mathrm{atan2}(m_{j,y}-y, m_{j,x}-x) - \theta $
\Return \Call{prob}{$r_t^i-\hat r, \sigma_r$}$\cdot$ \Call{prob}{$\phi_t^i-\hat \phi, \sigma_\phi$}$\cdot$ \Call{prob}{$s_t^i-\hat s_j, \sigma_s$}
\EndProcedure
\Procedure{prop-normal-distribution}{$q,\sigma$}
\Return $\frac{1}{\sqrt{2\pi \sigma}}\exp\left(-\frac{1 q}{2\sigma}\right)$
\EndProcedure
\Procedure{prop-triangle-distribution}{$a,b$}
\Return $\max\left\{0, \frac{1}{b\sqrt{6}} - \frac{|a|}{6b}\right\}$
\EndProcedure
\end{algorithmic}
\end{algorithm}
Hier sehen wir die Verteilung gegeben ein Landmark wurde mit Entfernung 5m und Peilung 30° gemessen. Je dunkler desto wahrscheinlicher ist die pose.
Sampling
Wenn wir gegeben eine Wahrnehmung bzw. Merkmal $f_t^i$ und der Korrespondenzvariable $c_t^i$ eine Stichprobe an wahrscheinlichen Roboterpose $x_t$ ziehen wenden wir folgende Algorithmus
\begin{algorithm}
\caption{Merkmalgestützes sensor model}
\begin{algorithmic}
\Input map $m$, feature $f_t^i$, correspondance variable $c_t^i$
\Procedure{sample-landmark-model-known-correspondance}{$f_t^i,c_t^i, x_t, m$}
\State $j \gets c_t^i$
\State $\hat\gamma \gets$ \Call{rand}{$0,2\pi$}
\State $\hat r \gets r_t^i +$ \Call{sample}{$\sigma_r$}
\State $\hat \phi \gets \phi_t^i +$ \Call{sample}{$\sigma_\phi$}
\State $x \gets m_{j,x} + \hat r \cos \hat\gamma$
\State $y \gets m_{j,y} + \hat r \sin \hat\gamma$
\State $\theta \gets \hat\gamma - \pi - \hat\phi$
\Return $x_t = \left( \begin{array}{ccc}x &y & \theta \end{array} \right)^T $
\EndProcedure
\Procedure{sample-normal-distribution}{$\sigma$}
\Return $\frac{1}{2}\sum_{i=1}^{12}\textrm{rand}(-\sigma,\sigma)$
\EndProcedure
\Procedure{sample-triangle-distribution}{$b$}
\Return $\frac{\sqrt{6}}{2}(\textrm{rand}(-\sqrt{b},\sqrt{b})+ \textrm{rand}(-\sqrt{b},\sqrt{b}))$
\EndProcedure
\end{algorithmic}
\end{algorithm}
Man sieht in eine Stichprobe an von Roboterpose gegeben ein Landmark wurde mit Entfernung 5m und Peilung 30° gemessen. Die Line des Kreise kennzeichnet die Roboterpeilung
