# The State Space Subdivision Filter for Estimation on SE(2)

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Density Representation and Approximation

- (A1)
- The function value in each area is identical to the corresponding grid value, i.e., $\forall \alpha \in {A}_{i}:{f}^{\omega}\left(\alpha \right)={\gamma}_{i}$.
- (A2)
- The conditional density ${f}^{\mathrm{c}}\left({\underline{x}}^{\tau}\right|\alpha )$ is the same for all $\alpha \in {A}_{i}$ (we denote it by ${f}_{i}^{\mathrm{c}}\left({\underline{x}}^{\tau}\right)$).
- (A3)
- The conditional density ${f}^{\mathrm{c}}\left({\underline{x}}^{\tau}\right|\alpha )$ is Gaussian for every considered $\alpha $.

#### 2.1. Deriving Parameters for a Given Density

#### 2.2. Normalization

#### 2.3. Providing Continuous Densities

## 3. Filter Derivation

#### 3.1. Update Step

#### 3.2. Prediction Step

- (B1)
- ${f}^{\mathrm{T},\omega}\phantom{\rule{-1.00374pt}{0ex}}\left({x}_{t+1}^{\omega}\right|{x}_{t}^{\omega})={f}^{\mathrm{T},\omega ,\mathrm{full}}\phantom{\rule{-1.00374pt}{0ex}}\left({x}_{t+1}^{\omega}\right|{\underline{x}}_{t}^{\tau},{x}_{t}^{\omega})$ holds for all ${\underline{x}}_{t}^{\tau}\in {\mathbb{R}}^{2}$.
- (B2)
- The function value of ${f}^{\mathrm{T},\omega}\phantom{\rule{-1.00374pt}{0ex}}\left({x}_{t+1}^{\omega}\right|{x}_{t}^{\omega})$ in each area in $\mathbb{S}\times \mathbb{S}$ is identical to the grid value for this area, i.e., ${f}^{\mathrm{T},\omega}\phantom{\rule{-1.00374pt}{0ex}}\left({\alpha}_{1}\right|{\alpha}_{2})={f}^{\mathrm{T},\omega}\phantom{\rule{-1.00374pt}{0ex}}\left({\beta}_{i}\right|{\beta}_{j})={\gamma}_{i,j}^{\mathrm{T}}$ for all ${\alpha}_{1}\in {A}_{i}$, ${\alpha}_{2}\in {A}_{j}$.
- (B3)
- The conditional density ${f}^{\mathrm{T},\mathrm{c},\mathrm{full}}\left({\underline{x}}_{t+1}^{\tau}\right|{\alpha}_{1},{\underline{x}}_{t}^{\tau},{\alpha}_{2})$ is the same function for all ${\alpha}_{1}\in {A}_{i}$, ${\alpha}_{2}\in {A}_{j}$ (we denote it by ${f}_{i,j}^{\mathrm{T},\mathrm{c}}\left({\underline{x}}_{t+1}^{\tau}\right|{\underline{x}}_{t}^{\tau})$).
- (B4)
- Each conditional density can be written as$${f}_{i,j}^{\mathrm{T},\mathrm{c}}\left({\underline{x}}_{t+1}^{\tau}\right|{\underline{x}}_{t}^{\tau})={f}_{\mathrm{N}}\left({\underline{x}}_{t+1}^{\tau};{\mathbf{F}}_{t,i,j}{\underline{x}}_{t}^{\tau}+{\underline{\widehat{u}}}_{t,i,j},{\mathbf{C}}_{t,i,j}^{\underline{w}}\right)$$

## 4. Evaluation

#### 4.1. Scenario Description

#### 4.2. Models Used by the Filters

#### 4.3. Evaluation Metrics

#### 4.4. Evaluation Results

#### 4.4.1. Evaluation Considering All Time Steps

#### 4.4.2. Evaluation of the Last Estimates and Run Times

## 5. Discussion, Summary, and Outlook

#### 5.1. Discussion

#### 5.2. Summary

#### 5.3. Outlook

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Conflicts of Interest

## References

- Tayefi, M.; Geng, Z.; Peng, X. Coordinated Tracking for Multiple Nonholonomic Vehicles on SE(2). Nonlinear Dyn.
**2017**, 87, 665–675. [Google Scholar] [CrossRef] - Triggs, B. Motion Planning for Nonholonomic Vehicles: An Introduction. 2010. inria-00548415. Available online: https://hal.inria.fr/inria-00548415 (accessed on 17 August 2021).
- Hess, W.; Kohler, D.; Rapp, H.; Andor, D. Real-Time Loop Closure in 2D LIDAR SLAM. In Proceedings of the 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, 16–21 May 2016; pp. 1271–1278. [Google Scholar]
- Chirikjian, G.S. Stochastic Models, Information Theory, and Lie Groups, Volume 2; Birkhäuser Boston: Boston, MA, USA, 2012. [Google Scholar]
- Adams, R.M.; Biggs, R.; Remsing, C.C. Two-Input Control Systems on the Euclidean Group SE(2). ESAIM Control Optim. Calc. Var.
**2013**, 19, 947–975. [Google Scholar] [CrossRef] - Xuan, S.; Li, S.; Han, M.; Wan, X.; Xia, G.S. Object Tracking in Satellite Videos by Improved Correlation Filters with Motion Estimations. IEEE Trans. Geosci. Remote Sens.
**2019**, 58, 1074–1086. [Google Scholar] [CrossRef] - Li, X.R.; Jilkov, V.P. Survey of Maneuvering Target Tracking. Part I: Dynamic models. IEEE Trans. Aerosp. Electron. Syst.
**2003**, 39, 1333–1364. [Google Scholar] - Montemerlo, M.; Thrun, S.; Koller, D.; Wegbreit, B. FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem. In Proceedings of the AAAI National Conference on Artificial Intelligence (AAAI 2002), Edmonton, AB, Canada, 28 July–1 August 2002. [Google Scholar]
- Arulampalam, M.S.; Maskell, S.; Gordon, N.; Clapp, T. A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking. IEEE Trans. Signal Process.
**2002**, 50, 174–188. [Google Scholar] [CrossRef] [Green Version] - Li, T.; Sun, S.; Sattar, T.P.; Corchado, J.M. Fight Sample Degeneracy and Impoverishment in Particle Filters: A Review of Intelligent Approaches. Expert Syst. Appl.
**2014**, 41, 3944–3954. [Google Scholar] [CrossRef] [Green Version] - Kokkala, J.; Särkkä, S. On the (Non-)Convergence of Particle Filters with Gaussian Importance Distributions. In Proceedings of the 17th IFAC Symposium on System Identification (SYSID 2015), Beijing, China, 19–21 October 2015. [Google Scholar]
- Barrau, A.; Bonnabel, S. Invariant Kalman Filtering. Annu. Rev. Control. Robot. Auton. Syst.
**2018**, 1, 237–257. [Google Scholar] [CrossRef] - Brossard, M.; Barrau, A.; Bonnabel, S. A Code for Unscented Kalman Filtering on Manifolds (UKF-M). In Proceedings of the 2020 IEEE International Conference on Robotics and Automation (ICRA 2020), Paris, France, 31 May–31 August 2020; pp. 5701–5708. [Google Scholar]
- Li, K.; Kurz, G.; Bernreiter, L.; Hanebeck, U.D. Nonlinear Progressive Filtering for SE(2) Estimation. In Proceedings of the 21st International Conference on Information Fusion (Fusion 2018), ISIF, Cambridge, UK, 10–13 July 2018. [Google Scholar]
- Li, K.; Pfaff, F.; Hanebeck, U.D. Dual Quaternion Sample Reduction for SE(2) Estimation. In Proceedings of the 23rd International Conference on Information Fusion (Fusion 2020), Virtual Conference, Sun City, South Africa, 6–9 July 2020. [Google Scholar]
- Murphy, K.; Russell, S. Rao–Blackwellized Particle Filtering for Dynamic Bayesian Networks. In Sequential Monte Carlo Methods in Practice; Springer: New York, NY, USA, 2001; pp. 499–515. [Google Scholar]
- Matoušek, J.; Duník, J.; Straka, O. Density Difference Grid Design in a Point-Mass Filter. Energies
**2020**, 13, 4080. [Google Scholar] [CrossRef] - Kurz, G.; Pfaff, F.; Hanebeck, U.D. Application of Discrete Recursive Bayesian Estimation on Intervals and the Unit Circle to Filtering on SE(2). IEEE Trans. Ind. Inform.
**2018**, 14, 1197–1206. [Google Scholar] [CrossRef] - Pfaff, F.; Li, K.; Hanebeck, U.D. Fourier Filters, Grid Filters, and the Fourier-Interpreted Grid Filter. In Proceedings of the 22nd International Conference on Information Fusion (Fusion 2019), ISIF, Ottawa, ON, Canada, 2–5 July 2019. [Google Scholar]
- Pfaff, F.; Li, K.; Hanebeck, U.D. Estimating Correlated Angles Using the Hypertoroidal Grid Filter. In Proceedings of the 2020 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2020), Karlsruhe, Germany, 14–16 September 2020. [Google Scholar]
- Zygmund, A. Trigonometric Series, 3rd ed.; Cambridge University Press: Cambridge, UK, 2003; Volumes 1–2. [Google Scholar]
- Pfaff, F.; Li, K.; Hanebeck, U.D. The Spherical Grid Filter for Nonlinear Estimation on the Unit Sphere. In Proceedings of the 1st Virtual IFAC World Congress (IFAC-V 2020), Virtual Conference, Berlin, Germany, 11–17 July 2020. [Google Scholar]
- Pfaff, F.; Li, K.; Hanebeck, U.D. A Hyperhemispherical Grid Filter for Orientation Estimation. In Proceedings of the 23rd International Conference on Information Fusion (Fusion 2020), Virtual Conference, Rustenburg, South Africa, 6–9 July 2020. [Google Scholar]
- Kurz, G.; Gilitschenski, I.; Hanebeck, U.D. The Partially Wrapped Normal Distribution for SE(2) Estimation. In Proceedings of the 2014 IEEE International Conference on Multisensor Fusion and Information Integration (MFI 2014), Beijing, China, 28–29 September 2014. [Google Scholar]
- Mardia, K.V.; Jupp, P.E. Directional Statistics; John Wiley & Sons: Chichester, UK, 2000. [Google Scholar]
- Mahler, R.P.S. Statistical Multisource-Multitarget Information Fusion; Artech House, Inc.: Norwood, MA, USA, 2007. [Google Scholar]
- Crouse, D.F.; Willett, P.; Pattipati, K.; Svensson, L. A Look at Gaussian Mixture Reduction Algorithms. In Proceedings of the 14th International Conference on Information Fusion (Fusion 2011), Chicago, IL, USA, 5–8 July 2011; pp. 1–8. [Google Scholar]
- Kurz, G.; Gilitschenski, I.; Pfaff, F.; Drude, L.; Hanebeck, U.D.; Haeb-Umbach, R.; Siegwart, R.Y. Directional Statistics and Filtering Using libDirectional. arXiv
**2017**, arXiv:1712.09718. [Google Scholar] [CrossRef] [Green Version] - Brossard, M.; Bonnabel, S.; Condomines, J.P. Unscented Kalman Filtering on Lie Groups. In Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2017), Vancouver, BC, Canada, 24–28 September 2017. [Google Scholar]
- Julier, S.J.; Uhlmann, J.K. Unscented Filtering and Nonlinear Estimation. Proc. IEEE
**2004**, 92, 401–422. [Google Scholar] [CrossRef] [Green Version] - Gilitschenski, I.; Kurz, G.; Julier, S.J.; Hanebeck, U.D. A New Probability Distribution for Simultaneous Representation of Uncertain Position and Orientation. In Proceedings of the 17th International Conference on Information Fusion (Fusion 2014), Salamanca, Spain, 7–10 July 2014. [Google Scholar]
- Wang, Y.; Chirikjian, G.S. Nonparametric Second-order Theory of Error Propagation on Motion Groups. Int. J. Robot. Res.
**2008**, 27, 1258–1273. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Xie, L.; Soh, Y.C. Robust Kalman Filtering for Uncertain Systems. Syst. Control Lett.
**1994**, 22, 123–129. [Google Scholar] [CrossRef] - Moheimani, S.; Savkin, A.; Petersen, I. Robust Filtering, Prediction, Smoothing, and Observability of Uncertain Systems. IEEE Trans. Circuits Syst. Fundam. Theory Appl.
**1998**, 45, 446–457. [Google Scholar] [CrossRef] [Green Version] - Li, K.; Pfaff, F.; Hanebeck, U.D. Grid-Based Quaternion Filter for SO(3) Estimation. In Proceedings of the 2020 European Control Conference (ECC 2020), St. Petersburg, Russia, 12–15 May 2020. [Google Scholar]

**Figure 2.**Naïve approximation of the density shown in Figure 1 based on a subdivision of the state space. 10 areas are used in this example.

**Figure 4.**Continuous density obtained by the use of a trigonometric polynomial for the periodic part and distance-based mixtures of Gaussians for the linear part.

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2021 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 (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Pfaff, F.; Li, K.; Hanebeck, U.D.
The State Space Subdivision Filter for Estimation on SE(2). *Sensors* **2021**, *21*, 6314.
https://doi.org/10.3390/s21186314

**AMA Style**

Pfaff F, Li K, Hanebeck UD.
The State Space Subdivision Filter for Estimation on SE(2). *Sensors*. 2021; 21(18):6314.
https://doi.org/10.3390/s21186314

**Chicago/Turabian Style**

Pfaff, Florian, Kailai Li, and Uwe D. Hanebeck.
2021. "The State Space Subdivision Filter for Estimation on SE(2)" *Sensors* 21, no. 18: 6314.
https://doi.org/10.3390/s21186314