\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. 

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(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^{\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$.