# A Multiple-Model Particle Filter Fusion Algorithm for GNSS/DR Slide Error Detection and Compensation

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Related Work Materials and Methods

## 3. Error Model of the Odometry

_{odo}). We have tuned the value of σ

_{odo}to 0.26 m, corresponding with the odometry step in our vehicle setup.

^{slide}(k) ≥ 0 stands for the sliding error, that may be also developed as

_{odo}), and scv (slide compensation value) varying from 0 (maximum compensation) till 1 (minimum).

## 4. Multiple Model Particle Filter Based Method

^{i}(k)}

^{N}

_{{i=1}}and their corresponding weights {w

^{i}(k)}

^{N}

_{{i=1}}the probability density function p(

**X**(k)/Y

_{{1:k}}) at instant k of the state vector

**X**(k), given past observations, following the expression

_{{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 δ

^{i}(k)(

**X**(k)) in the form

- Initialization: Generation of N particles, or samples of the state vector, X
^{i}(0), with equal weights 1/N. The proposed state vector is $\left[x\left(k\right)\text{}y\left(k\right)\text{}\psi \left(k\right)\right]$, 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}\left(k\right)$, is processed. The equation for pose prediction is:$$x\left(k+1\right)=x\left(k\right)+ds\left(k\right)\mathrm{sin}\mathrm{c}(\dot{\psi}/2)\mathrm{cos}(\psi \left(k\right)+\dot{\psi}\left(k\right)T/2)-\dot{\psi}\left(k\right)T\left(Dx\mathrm{sin}\psi \left(k\right)+Dy\mathrm{cos}\psi \left(k\right)\right)\phantom{\rule{0ex}{0ex}}y\left(k+1\right)=y\left(k\right)+ds\left(k\right)\mathrm{sin}\mathrm{c}(\dot{\psi}/2)\mathrm{sin}\left(\psi \left(k\right)+\omega \left(k\right)T/2\right)-\dot{\psi}\left(k\right)T\left(Dx\mathrm{cos}\psi \left(k\right)-Dy\mathrm{sin}\psi \left(k\right)\right)\phantom{\rule{0ex}{0ex}}\psi \left(k+1\right)=\psi \left(k\right)+\dot{\psi}\left(k\right)T$$ - Measurement update: Update of the weights of the particles with the observations Y(k). In our case, the observation vector is $[{x}_{GPS},\text{}{y}_{GPS}]$, standing for east and north values coming from the GPS. The update is done following the expression$${w}^{i}\left(k\right)={w}^{i}\left(k-1\right)\times {e}^{\left\{-0.5\text{}{\left(Y\left(k\right)-h{X}^{i}\left(k\right)\right)}^{\prime}{R}^{-1}\left(Y\left(k\right)-h{X}^{i}\left(k\right)\right)\right\}}$$
^{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}\left(k\right)={w}^{i}\left(k\right)/{\sum}_{i=1}^{N}{w}^{i}\left(k\right)$.
- 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}{\left(k\right)}^{2}}<0.5N$$ - End of cycle: Making k = k + 1, and iterating to step 2.

^{m}(k). This probability is

_{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{\left(k\right)}_{i}^{\ne m}$ stands for the weight at instant k of a particle i that is not driven by model m.

## 5. Filter Consistency

#### 5.1. Filter Covariance

#### 5.2. Time Average Autocorrelation

#### 5.3. Time Average Normalized Innovation Square

## 6. Tests

#### 6.1. Test Setup

- EGNOS capable GPS receiver by Trimble, L1.
- Dual-frequency RTK receiver by Ashtech, for ground reference.
- FOG (Fiber Optic Gyroscope) by KVH.
- Vehicle odometer with 26.15 cm resolution coupled to the rear wheels axle.

#### 6.2. Results and Discussion

## 7. Conclusions

## Acknowledgments

## Author Contributions

## Conflicts of Interest

## References

- Bétaille, D.; Bonnifait, P.; Blanco Delgado, N.; Cosmen Schortmann, J.; Engdahl, J.; Gikas, V.; Gilliéron, P.Y.; Hodon, M.; Feng, S.; Machaj, J.; et al. SaPPART White Paper. Better Use of Global Navigation Satellite Systems for Safer and Greener Transport; IFSTTAR Techniques et Méthodes; IFSTTAR: Paris, France, 2015. [Google Scholar]
- Toledo, R.; Zamora, M.A.; Ubeda, B.; Gomez-Skarmeta, A.F. An Integrity Navigation System based on GNSS/INS for Remote Services Implementation in Terrestrial Vehicles. In Proceedings of the 7th International IEEE Conference on Intelligent Transportation Systems, Washington, DC, USA, 3–6 October 2004; pp. 477–480. [Google Scholar]
- Toledo-Moreo, R.; Bétaille, D.; Peyret, F. Lane-Level Integrity Provision for Navigation and Map Matching With GNSS, Dead Reckoning, and Enhanced Maps. IEEE Trans. Intell. Transp. Syst.
**2010**, 11, 100–112. [Google Scholar] [CrossRef] - Sukkarieh, S.; Durant-Whyte, H.; Nebot, E. A High Integrity IMU/GPS Navigation Loop for Autonomous Land Vehicle Applications. IEEE Trans. Robot. Autom.
**1999**, 15, 572–578. [Google Scholar] [CrossRef] - Wilson, C.; Wang, J. Safety at the Wheel. Improving KGPS/INS Performance and Reliability; GPS World: Duluth, MN, USA, 2003; pp. 16–26. [Google Scholar]
- Berdjag, D.; Pomorski, D. DGPS/INS data fusion for land navigation. In Proceedings of the Fusion 2004, Stockholm, Sweden, 28 June–1 July 2004. [Google Scholar]
- Zhang, P.; Gu, J.; Milios, E.E.; Huynh, P. Navigation with IMU/GPS/digital compass with unscented Kalman filter. In Proceedings of the IEEE International Conference on Mechatronics and Automation, Niagara Falls, ON, Canada, 29 July–1 August 2005; pp. 1497–1502. [Google Scholar]
- Liu, B.; Adam, M.; Ibañez-Guzman, J. Multi-aided Inertial Navigation for Ground Vehicles in Outdoor Uneven Environments. In Proceedings of the 2005 IEEE International Conference on Robotics and Automation, Barcelona, Spain, 18–22 April 2005. [Google Scholar]
- Toledo-Moreo, R.; Zamora-Izquierdo, M.; Ubeda, B.; Skarmeta, M.A. High-Integrity IMM-EKF-Based Road Vehicle. IEEE Trans. Intell. Transp. Syst.
**2007**, 8, 491–511. [Google Scholar] [CrossRef] - Takeyama, K.; Machida, T.; Kojima, Y.; Kubo, N. Improvement of Dead Reckoning in Urban Areas through Integration of Low-Cost Multisensors. IEEE Trans. Intell. Veh.
**2017**, 2, 278–287. [Google Scholar] [CrossRef] - Alonso, I.P.; Llorca, D.F.; Gavilan, M.; Pardo, S.Á.; Garcia-Garrido, M.A.; Vlacic, L.; Sotelo, M.Á. Accurate Global Localization Using Visual Odometry and Digital Maps on Urban Environments. IEEE Trans. Intell. Transp. Syst.
**2012**, 13, 1535–1545. [Google Scholar] [CrossRef] - Kunii, Y.; Kovacs, G.; Hoshi, N. Mobile robot navigation in natural environments using robust object tracking. In Proceedings of the IEEE 26th International Symposium on Industrial Electronics (ISIE), Edinburgh, UK, 19–21 June 2017; pp. 1747–1752. [Google Scholar]
- Chaves, G.D.L.; Martins, L.G.A.; de Oliveira, G.M.B. An improved model based on cellular automata for on-line navigation. In Proceedings of the 2017 Latin American Robotics Symposium (LARS) and 2017 Brazilian Symposium on Robotics (SBR), Curitiba, Brazil, 8–11 November 2017. [Google Scholar]
- Bar-Shalom, Y. Estimation and Tracking: Principles; Artech House: Nonvood, MA, USA, 1993. [Google Scholar]
- Skog, I.; Handel, P. In-Car Positioning and Navigation Technologies—A Survey. IEEE Trans. Intell. Transp. Syst.
**2009**, 10, 4–21. [Google Scholar] [CrossRef] - Doucet, A.; de Freitas, N.; Gordon, N. Sequential Monte Carlo; Springer: New York, NY, USA, 2001. [Google Scholar]
- Djuric, P.M.; Kotecha, J.H.; Zhang, J.; Huang, Y.; Ghirmai, T.; Bugallo, M.F.; Miguez, J. Particle filtering. IEEE Signal Proceess. Mag.
**2003**, 20, 19–38. [Google Scholar] [CrossRef] - Toledo-Moreo, R.; Zamora-Izquierdo, M. IMM-Based Lane-Change Prediction in Highways with Low-Cost GPS/INS. IEEE Trans. Intell. Transp. Syst.
**2009**, 10, 180–185. [Google Scholar] [CrossRef] - Ekman, M.; Sviestins, E. Multiple Model Algorithm Based on Particle Filters for Ground Target Tracking. In Proceedings of the 2007 10th International Conference on Information Fusion, Quebec, QC, Canada, 9–12 July 2007; pp. 1–8. [Google Scholar]
- Hong, L.; Cui, N.; Bakich, M.; Layne, J.R. Multirate interacting multiple model particle filter for terrain-based ground target tracking. IEE Proc. Control Theory Appl.
**2006**, 53, 721–731. [Google Scholar] [CrossRef] - Guo, R.; Qin, Z.; Li, X.; Chen, J. IMMUPF Method for Ground Target Tracking. In Proceedings of the IEEE International Conference on Systems, Man and Cybernetics, Montreal, QC, Canada, 7–10 October 2007; pp. 96–101. [Google Scholar]
- Foo, P.H.; Ng, G.W. Combining IMM Method with Particle Filters for 3D Maneuvering Target Tracking. In Proceedings of the 2007 10th International Conference on Information Fusion, Quebec, QC, Canada, 9–12 July 2007; pp. 1–8. [Google Scholar]
- Wang, J.; Zhao, D.; Gao, W.; Shan, S. Interacting Multiple Model Particle Filter to Adaptive Visual Tracking. In Proceedings of the Image and Graphics (ICIG’04), Hong Kong, China, 18–20 December 2004; pp. 568–571. [Google Scholar]
- Giremus, A.; Tourneret, J.-Y.; Calmettes, V. A Particle Filtering Approach for Joint Detection/Estimation of Multipath Effects on GPS Measurements. IEEE Trans. Signal Process.
**2007**, 55, 1275–1285. [Google Scholar] [CrossRef] [Green Version]

Model | Value |
---|---|

NCMPF | 0.5762 |

SCMPF | 0.9110 |

MMPF | 0.0140 |

© 2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Toledo-Moreo, R.; Colodro-Conde, C.; Toledo-Moreo, J.
A Multiple-Model Particle Filter Fusion Algorithm for GNSS/DR Slide Error Detection and Compensation. *Appl. Sci.* **2018**, *8*, 445.
https://doi.org/10.3390/app8030445

**AMA Style**

Toledo-Moreo R, Colodro-Conde C, Toledo-Moreo J.
A Multiple-Model Particle Filter Fusion Algorithm for GNSS/DR Slide Error Detection and Compensation. *Applied Sciences*. 2018; 8(3):445.
https://doi.org/10.3390/app8030445

**Chicago/Turabian Style**

Toledo-Moreo, Rafael, Carlos Colodro-Conde, and Javier Toledo-Moreo.
2018. "A Multiple-Model Particle Filter Fusion Algorithm for GNSS/DR Slide Error Detection and Compensation" *Applied Sciences* 8, no. 3: 445.
https://doi.org/10.3390/app8030445