192 lines
11 KiB
TeX
192 lines
11 KiB
TeX
\section{IMMPF and Mixing}
|
|
\label{sec:immpf}
|
|
|
|
%Einführen des IMMPF
|
|
Consider a jump Markov non-linear system that is represented by different particle filters as state space description, where the characteristics change in time according to a Markov chain.
|
|
The posterior distribution is then described by
|
|
%
|
|
\begin{equation}
|
|
p(\mStateVec_{t}, m_t \mid \mObsVec_{1:t}) = P(m_t \mid \mObsVec_{1:t}) p(\mStateVec_{t} \mid m_t, \mObsVec_{1:t})
|
|
\label{equ:immpfPosterior}
|
|
\end{equation}
|
|
%
|
|
where $m_t\in M\subset \mathbb{N}$ is the modal state of the system and thus describes the current mode (particle filter) \cite{Driessen2005}.
|
|
The notation $P(\cdot)$ provides a discrete probability distribution.
|
|
Given \eqref{equ:immpfPosterior} and \eqref{equ:bayesInt}, the mode conditioned filtering stage can be written as
|
|
%
|
|
\begin{equation}
|
|
\arraycolsep=1.2pt
|
|
\begin{array}{ll}
|
|
p(\mStateVec_{t} \mid m_t, \mObsVec_{1:t}) \propto
|
|
&p(\mObsVec_{t} \mid m_t, \mStateVec_{t})\\
|
|
&\int p(\mStateVec_{t} \mid \mStateVec_{t-1}, m_t, \mObsVec_{t-1})\\
|
|
&p(\mStateVec_{t-1} \mid m_{t-1}, \mObsVec_{1:t-1})d\vec{q}_{t-1}
|
|
|
|
\end{array}
|
|
\label{equ:immpfFiltering}
|
|
\end{equation}
|
|
%
|
|
and the posterior mode probabilities, providing how likely it is that a considered mode represents the system's state, are calculated by
|
|
%
|
|
\begin{equation}
|
|
p(m_t \mid \mObsVec_{1:t}) \propto p(\mStateVec_{t} \mid m_t, \mObsVec_{1:t-1}) P(m_t \mid \mObsVec_{1:t-1})
|
|
\enspace .
|
|
\label{equ:immpModeProb}
|
|
\end{equation}
|
|
%
|
|
It should be noted that \eqref{equ:immpfFiltering} and \eqref{equ:immpModeProb} are not normalized and thus such a step is required.
|
|
To provide a solution for the probability distribution $P(m_t \mid \mObsVec_{1:t-1})$, the recursion for $m_t$ in \eqref{equ:immpfPosterior} is now derived by the so called mixing stage \cite{Driessen2005}.
|
|
Here, we compute
|
|
%
|
|
\begin{equation}
|
|
\arraycolsep=1.2pt
|
|
\begin{split}
|
|
&p(\mStateVec_{t} \mid m_{t+1}, \mObsVec_{1:t}) = \\
|
|
& \sum_{m_t} P(m_t \mid m_{t+1}, \mObsVec_{1:t}) p(\mStateVec_{t} \mid m_t, \mObsVec_{1:t})
|
|
\end{split}
|
|
\label{equ:immpModeMixing}
|
|
\end{equation}
|
|
%
|
|
with
|
|
%
|
|
\begin{equation}
|
|
P(m_t \mid m_{t+1}, \mObsVec_{1:t}) = \frac{P(m_{t+1} \mid m_t) P(m_t \mid \mObsVec_{1:t})}{P(m_t \mid \mObsVec_{1:t-1})}
|
|
\label{equ:immpModeMixing2}
|
|
\end{equation}
|
|
%
|
|
and
|
|
%
|
|
\begin{equation}
|
|
P(m_t \mid \mObsVec_{1:t-1}) = \sum_{m_t}{P(m_{t+1} \mid m_t) P(m_t \mid \mObsVec_{1:t})}
|
|
\enspace ,
|
|
\label{equ:immpModeMixing3}
|
|
\end{equation}
|
|
%
|
|
where \eqref{equ:immpModeMixing} is a weighted sum of distributions and the weights are provided through \eqref{equ:immpModeMixing2}.
|
|
To find a solution for $P(m_t \mid \mObsVec_{1:t})$, an estimate of the posterior probability $p(m_t \mid \mObsVec_{1:t})$ in \eqref{equ:immpModeProb} can be calculated according to
|
|
%
|
|
\begin{equation}
|
|
P(m_t \mid \mObsVec_{1:t}) = \frac{\omega_t^{m_t} P(m_t \mid \mObsVec_{1:t-1})}{\sum_{m=1}^M \omega_t^{m_t} P(m_t \mid \mObsVec_{1:t-1})}
|
|
\enspace .
|
|
\label{equ:immpMode2}
|
|
\end{equation}
|
|
%
|
|
Here, $\omega_t^{m_t}$ is the unnormalized weight given by the state evaluation of the respective mode $m_t$.
|
|
The initial mode probabilities $P(m_1 \mid \mObsVec_{1})$ have to be defined beforehand.
|
|
The transition probability $P(m_{t+1} = k \mid m_t = l)$ in \eqref{equ:immpModeMixing3} is given by the Markov transition matrix $[\Pi_t]_{kl}$.
|
|
The matrix $\Pi_t$ is a real square matrix, with each row summing to 1.
|
|
It provides the probability of moving from $m_t$ to $m_{t+1}$ in one time step.
|
|
Sampling from \eqref{equ:immpModeMixing} is done by first drawing a modal state $m_t$ from $P(m_t \mid m_{t+1}, \mObsVec_{1:t})$ and then drawing a state $\mStateVec_{t}$ from $p(\mStateVec_{t} \mid m_t, \mObsVec_{1:t})$ in regard to that $m_t$.
|
|
In context of particle filtering, this means that \eqref{equ:immpModeMixing} enables us to pick particles from all available modes in regard to the discrete distribution $P(m_t \mid m_{t+1}, \mObsVec_{1:t})$.
|
|
Further, the number of particles in each mode can be selected independently of the actual mode probabilities.
|
|
|
|
Algorithm \ref{alg:immpf} shows the complete IMMPF procedure in detail.
|
|
As prior knowledge, $M$ initial probabilities $P(m_1 \mid \mObsVec_{1})$ and initial distributions $p(\mStateVec_{1} \mid m_1, \mObsVec_{1})$ providing a particle set $\{W^i_{1}, \vec{X}^i_{1} \}_{i=1}^N$ are available.
|
|
The mixing step requires that the independently running filtering processes are all finished.
|
|
|
|
\begin{algorithm}[t]
|
|
\caption{IMMPF Algorithm}
|
|
\label{alg:immpf}
|
|
\begin{algorithmic}[1] % The number tells where the line numbering should start
|
|
\Statex{\textbf{Input:} Prior $P(m_1 \mid \mObsVec_{1})$ and $p(\mStateVec_{1} \mid m_1, \mObsVec_{1})$}
|
|
\Statex{~}
|
|
\For{$m_t = 0$ \textbf{to} $M$} \Comment{Mixing}
|
|
\For{$i = 0$ \textbf{to} $N_{m_t}$}
|
|
\State Sample $m^i_{t-1} \sim P(m_{t-1} \mid m_{t}, \mObsVec_{1:t-1})$
|
|
\State Sample $\vec{X}^{i, m_t}_{t-1} \sim p(\mStateVec_{t-1} \mid m^i_{t-1}, \mObsVec_{1:t-1})$
|
|
\State Set $W^{i, m_t}_{t-1}$ to $\frac{1}{N_{m_t}}$
|
|
\EndFor
|
|
\EndFor
|
|
\Statex{~}
|
|
\Statex \textbf{Run:} Parallel filtering for each $m_t \in M$ \Comment{Filtering}
|
|
\For{$i = 0$ \textbf{to} $N_{m_t}$}
|
|
\State Sample $\vec{X}_t^{i,m_t} \sim p(\vec{X}_t^{i,m_t} \mid \vec{X}_{t-1}^{i,m_t}, \mObsVec_{t-1})$\Comment{Transition}
|
|
\State Compute $W^{i,m_t}_t \propto p(\vec{o}_t \mid \vec{X}_{t}^{i, m_t})$ \Comment{Evaluation}
|
|
\EndFor
|
|
\State Calculate $\omega_t^{m_t} = \sum_{i=1}^{N_{m_t}} W^{i, m_t}_t$
|
|
\State Normalise $W^{i,m_t}_t$ using $\omega_t^{m_t}$
|
|
\State Resample $\{W_{t}^{i,m_t}, \vec{X}_{t}^{i,m_t} \}$ to obtain $N_{m_t}$ new equally-weighted particles $\{\frac{1}{N_{m_t}}, \overline{\vec{X}}_{t}^{i,m_t} \}$
|
|
\vspace{0.1cm}
|
|
\State Estimate $P(m_t \mid \mObsVec_{1:t}) = \frac{\omega_t^{m_t} P(m_t \mid \mObsVec_{1:t-1})}{\sum_{m=1}^M \omega_t^{m_t} P(m_t \mid \mObsVec_{1:t-1})}$
|
|
\end{algorithmic}
|
|
\end{algorithm}
|
|
|
|
|
|
%grundidee warum die matrix so gewählt wird.
|
|
With the above, we are finally able to combine the two filters described in section \ref{sec:rse} and realize the considerations made in section \ref{sec:divergence}.
|
|
Within the IMMPF we utilize the restrictive graph-based filter as the \textit{dominant} one, providing the state estimation for the localisation.
|
|
Due to its robustness and good diversity the simple, more permissive filter, is then used as \textit{support} for possible sample impoverishment.
|
|
The names dominant and support are now applied as synonyms for the respective filters used as modes within the IMMPF.
|
|
|
|
As a reminder, both filters (modes) are running in parallel for the entire estimation life cycle.
|
|
If we recognize that the dominant filter diverges from the supporting filter and thus got stuck or lost track, particles from the supporting filter will be picked with a higher probability while mixing the new particle set for the dominant filter.
|
|
As seen before, the Markov transition matrix $\Pi_t$ provides the probability $P(m_{t+1} \mid m_t)$ for transitioning between modes.
|
|
%In our approach those modes are the dominant graph-based filter and the supporting simple filter.
|
|
The dominant filter's probability to draw particles from its own posterior is given by the positive exponential distribution
|
|
%
|
|
\begin{equation}
|
|
f(D_{\text{KL}}, \lambda) = e^{-\lambda D_{\text{KL}}}
|
|
\enspace .
|
|
\label{equ:KLD}
|
|
\end{equation}
|
|
%
|
|
Therefore, drawing particles from the support is given by $1 - f(D_{\text{KL}}, \lambda)$.
|
|
If the Kullback-Leibler divergence $D_{\text{KL}}$ increases to a certain point, \eqref{equ:KLD} provides a probability that allows for mixing the particle sets.
|
|
$\lambda$ depends highly on the respective filter models and is therefore chosen heuristically.
|
|
In most cases $\lambda$ tends to be somewhere between $0.01$ and $0.10$.
|
|
|
|
However, \eqref{equ:KLD} only works reliable if the measurement noise is within reasonable limits, because the support filter using the simple transition depends solely on them.
|
|
Especially \docWIFI{} serves as the main source for estimation and thus attenuated or bad \docWIFI{} readings are causing bad estimation results for the supporting filter.
|
|
This further leads to a growing $D_{\text{KL}}$, even if the dominant filter provides a good position estimation.
|
|
In such scenarios a lower diversity and higher focus of the particle set, as given by the dominant filter, is required.
|
|
We achieves this by introducing a \docWIFI{} quality factor, allowing the support filter to pick particles from the dominant filter and prevent the later from doing it vice versa.
|
|
The quality factor is defined by
|
|
%
|
|
\begin{equation}
|
|
\newcommand{\leMin}{l_\text{min}}
|
|
\newcommand{\leMax}{l_\text{max}}
|
|
q(\mObsVec_t^{\mRssiVec_\text{wifi}}) =
|
|
\max \left(0,
|
|
\min \left(
|
|
\frac{
|
|
\bar\mRssi_\text{wifi} - \leMin
|
|
}{
|
|
\leMax - \leMin
|
|
},
|
|
1
|
|
\right)
|
|
\right)
|
|
%,\enskip
|
|
%\bar\mRssi_\text{wifi} = \frac{1}{n} \sum_{i = 1}^{n} \mRssi_i
|
|
\label{eq:wifiQuality}
|
|
\end{equation}
|
|
%
|
|
where $\bar\mRssi_\text{wifi}$ is the average of all signal strength measurements received from the observation $\mObsVec_t^{\mRssiVec_\text{wifi}}$. An upper and lower bound is given by $l_\text{max}$ and $l_\text{min}$.
|
|
|
|
%matrix
|
|
To incorporate all this within the IMMPF, we utilize a non-trivial Markov switching process.
|
|
This is done by updating the Markov transition matrix $\Pi_t$ at every time step $t$.
|
|
As reminder, $\Pi_t$ highly influences the mixing process in \eqref{equ:immpModeMixing2} and is responsible for providing the probability of moving from $m_t$ to $m_{t+1}$ in one time step.
|
|
Considering the measures \eqref{equ:KLD} and \eqref{eq:wifiQuality} presented above, the $2$x$2$ matrix $\Pi_t$ is given by
|
|
%
|
|
\begin{equation}
|
|
\Pi_t =
|
|
\begin{pmatrix}
|
|
f(D_{\text{KL}}, \lambda) & 1 - f(D_{\text{KL}}, \lambda) \\
|
|
1 - q(\mObsVec_t^{\mRssiVec_\text{wifi}}) & q(\mObsVec_t^{\mRssiVec_\text{wifi}})\\
|
|
\end{pmatrix}
|
|
\enspace ,
|
|
\label{equ:immpMatrix}
|
|
\end{equation}
|
|
%
|
|
This matrix is the centrepiece of our approach.
|
|
It is responsible for controlling and satisfying the need of diversity and the need of focus for the whole localisation approach.
|
|
How $\Pi_t$ works is straightforward.
|
|
If the dominant graph-based filter suffers from sample impoverishment (get stuck or lose track), the probability in $[\Pi_t]_{12} = (1 - f(D_{\text{KL}}))$ increases with diverging support filter.
|
|
Consequently, the number of particles, the dominant filter draws from the supporting filter, also increases by $[\Pi_t]_{12} \cdot 100\%$.
|
|
Similar behaviour applies to the \docWIFI{} quality factor $q(\mObsVec_t^{\mRssiVec_\text{wifi}})$.
|
|
If the quality is low, the supporting filter regains focus by obtaining particles from the dominant's posterior.
|
|
Therefore, recovering from sample impoverishment or degeneracy depends to a large extent on $\Pi_t$.
|
|
|
|
|