175 lines
9.5 KiB
TeX
175 lines
9.5 KiB
TeX
\section{IMMPF and Mixing}
|
|
\label{sec:immpf}
|
|
|
|
In the previous section we have introduced a standard particle filter, an evaluation step and two different transition models.
|
|
Using this, we are able to implement two different localisation schemes.
|
|
One providing a high diversity with a robust, but uncertain position estimation.
|
|
The other keeps the localisation error low by using a very realistic propagation model, while being prone to sample impoverishment \cite{Ebner-15}.
|
|
In the following, we will combine those filters using the Interacting Multiple Model Particle Filter (IMMPF) and a non-trivial Markov switching process.
|
|
|
|
%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_k \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 \cite{Driessen2005}.
|
|
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 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 $P(m_t \mid \mObsVec_{1:t-1})$, the recursion for $m_t$ in \eqref{equ:immpfPosterior} is now derived by the 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}.
|
|
The transition probability $P(m_{t+1} = k \mid m_t = l)$ is given by the Markov transition matrix $[\Pi_t]_{kl}$.
|
|
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 dependence 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 independent running filtering process 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}.
|
|
The basic idea of our approach is to utilize the restrictive filter as the dominant one, providing the state estimation for the localisation.
|
|
Due to its robustness and good diversity the other, more permissive one, is then used as support for possible sample impoverishment.
|
|
If we recognize that the dominant filter gets stuck or loses track, particles from the supporting filter will be picked with a higher probability while mixing the new particle set for the dominant filter.
|
|
|
|
%kld
|
|
This is achieved by measuring the Kullback-Leibler divergence $D_{\text{KL}}(P \|Q)$ between both filtering posterior distributions $p(\mStateVec_{t} \mid m_t, \mObsVec_{1:t})$.
|
|
The Kullback-Leibler divergence is a non-symmetric non-negative difference between two probability distributions $Q$ and $P$.
|
|
It is also stated as the amount of information lost when $Q$ is used to approximate $P$ \cite{Sun2013}.
|
|
We set $D_{\text{KL}} = D_{\text{KL}}(P \|Q)$, while $P$ is the dominant and $Q$ the supporting filter.
|
|
Since the supporting filter is more robust and ignores all environmental restrictions, we are able to make a statement whether state estimation is stuck due to sample impoverishment or not by looking at the positive exponential distribution
|
|
%
|
|
\begin{equation}
|
|
f(D_{\text{KL}}, \lambda) = e^{-\lambda D_{\text{KL}}}
|
|
\label{equ:KLD}
|
|
\end{equation}
|
|
%
|
|
If $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, it is obvious that \eqref{equ:KLD} only works reliable if the measurement noises are within reasonable limits, because the support filter depends solely on them.
|
|
Especially Wi-Fi serves as the main source for estimation and thus attenuated or bad Wi-Fi readings are causing $D_{\text{KL}}$ to grow, 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 Wi-Fi 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(0,
|
|
\min(
|
|
\frac{
|
|
\bar\mRssi_\text{wifi} - \leMin
|
|
}{
|
|
\leMax - \leMin
|
|
},
|
|
1
|
|
)
|
|
)
|
|
\label{eq:wifiQuality}
|
|
\end{equation}
|
|
%
|
|
where $\bar\mRssi_\text{wifi}$ is the average of all signal-strength measurements received from the observation $\mObsVec_t$. An upper and lower bound is given by $l_\text{max}$ and $l_\text{min}$.
|
|
|
|
|
|
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}.
|
|
Considering the above presented measures, $\Pi_t$ is two-dimensional and 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 localization approach.
|
|
Therefore, recovering from sample impoverishment or degeneracy depends to a large extent on $\Pi_t$.
|
|
|