## 4 Multiple Model Particle Filter Based Method  
The principle of the particle filter is to represent by means of a set of \(N\) samples \(\{X^{i}(k)\}^{N}_{\{i=1\}}\) and their corresponding weights \(\{w^{i}(k)\}^{N}_{\{i=1\}}\) the probability density function \(p(\mathbf{X}(k)/Y_{\{1:k\}})\) at instant \(k\) of the state vector \(\mathbf{X}(k)\), given past observations, following the expression  
\[p(\mathbf{X}(k)/Y_{\{1:k\}})\simeq\sum_{i=1}^{N}X^{i}(k)\cdot w^{i}(k) \tag{3}\]  
where \(Y_{\{1:k\}}\) stands for the observations collected from the initialization till instant \(k\). Each sample \(X^{i}(k)\) can be described as a Dirac delta expression \(\delta^{i}(k)(\mathbf{X}(k))\) in the form  
\[\delta^{i}(k)(\mathbf{X}(k))=0\;if\;\mathbf{X}(k)\
eq X^{i}(k) \tag{4}\]  
\[\delta^{i}(k)(\mathbf{X}(k))=1\;if\;\mathbf{X}(k)=X^{i}(k) \tag{5}\]  
The process of particle filtering can be summarized as follows:  
* Initialization: Generation of \(N\) particles, or samples of the state vector, \(X^{i}(0)\), with equal weights \(1/N\). The proposed state vector is \([x(k)\;y(k)\;\psi(k)]\), representing east, north and heading (from north to east) at the center of the rear axle of the vehicle. * Prediction: Estimation of \(X^{i}(k+1)\) following the prediction model. We use a classical 2D kinematical model for a vehicle on a plane. The measurements of the odometry and the gyroscope work as inputs to the filter. The travelled distance measured by the odometer, \(ds(k)\), is estimated when a gyroscope value, \(\dot{\psi}(k)\), is processed. The equation for pose prediction is: \[ \begin{split} x(k+1)&=x(k)+ds(k)\sin c(\dot{\psi} /2)\cos\left(\psi(k)+\dot{\psi}(k)T/2\right)-\dot{\psi}(k)T(Dx\sin\psi(k)+Dy \cos\psi(k))\\ y(k+1)&=y(k)+ds(k)\sin c(\dot{\psi}/2)\sin(\psi(k)+ \omega(k)T/2)-\dot{\psi}(k)T(Dx\cos\psi(k)-Dy\sin\psi(k))\\ &\psi(k+1)=\psi(k)+\dot{\psi}(k)T\end{split} \] (6) being \(T\) the sampling period, \(Dx\), \(Dy\) the coordinate of the GPS antenna in the body frame, and sinc the cardinal sinus. * Measurement update: Update of the weights of the particles with the observations \(Y(k)\). In our case, the observation vector is \([x_{GPS},\ y_{GPS}]\), standing for east and north values coming from the GPS. The update is done following the expression \[w^{i}(k)=w^{i}(k-1)\times e^{\left\{-0.5\ \left(Y(k)-hX^{i}(k)\right)^{ \prime}R^{-1}\left(Y(k)-hX^{i}(k)\right)\right\}}\] (7) where \(h(X^{i}(k))\) is the observation function that relates at instant \(k\) the state \(X^{i}(k)\) and observations \(Y(k)\) (in our case, simply the second order identity matrix), and \(R\) the covariance matrix of observations. * Normalization of the weights: \(w^{i}(k)=w^{i}(k)/\sum_{i=1}^{N}w^{i}(k)\). * Resampling: To prevent high concentration of probability mass in only a few particles, (leading to the convergence of a single \(w^{i}(k)\) to 1), particles are resampled if \[\frac{1}{\sum_{i=1}^{N}w^{i}(k)^{2}}<0.5N\] (8)
* End of cycle: Making \(k=k+1\), and iterating to step 2. In our approach, a fixed number of particles is assigned to the each model. The addition of the weights of the particles of each model after the observation will indicate the probability that each model \(m\) represents properly the vehicle behavior, \(\mu^{m}(k)\). This probability is  
\[\mu^{m}(k)=\frac{\sum_{i}^{Nm}w(k)_{i}^{m}}{\sum_{i}^{Nm}w(k)_{i}^{m}+\sum_{i} ^{Nm}w(k)_{i}^{\
eq m}} \tag{9}\]  
where \(w(k)_{i}^{m}\) stands for the weight at instant \(k\) of the particle \(i\) that represents model \(m\), \(Nm\) is the number of particles of each model, that will be equal for all of them, and \(w(k)_{i}^{\
eq m}\) stands for the weight at instant \(k\) of a particle \(i\) that is not driven by model \(m\). Our solution is based on two different models, named the normal condition model (NCM) and the slide condition model (SCM). Both models will actually share the state vector and prediction equation, but will have different assumptions for the odometry model with and without slides, as described in (1).