Feedback Particle Filter on Riemannian Manifolds and Matrix Lie Groups

This paper is concerned with the problem of continuous-time nonlinear filtering of stochastic processes evolving on connected Riemannian manifolds without boundary. The main contribution of this paper is to derive the feedback particle filter (FPF) algorithm for this problem. In its general form, the FPF is shown to provide an intrinsic description of the filter that automatically satisfies the geometric constraints of the manifold. The particle dynamics are encapsulated in a Stratonovich stochastic differential equation that retains the feedback structure of the original (Euclidean) FPF. The implementation of the filter requires a solution of a Poisson equation on the manifold, and a numerical algorithm is described for this purpose. For the special case when the manifold is a matrix Lie group, explicit formulae for the filter are derived, using the matrix coordinates. Filters for two example problems are presented: the attitude estimation problem on $SO(3)$ and the robot localization problem in $SE(3)$ . Comparisons are also provided between the FPF and popular algorithms for attitude estimation, namely the multiplicative extended Kalman filter (EKF), the invariant EKF, the unscented quaternion estimator, the invariant ensemble Kalman filter, and the bootstrap particle filter. Numerical simulations are presented to illustrate these comparisons.

[1]  Sean P. Meyn,et al.  Feedback Particle Filter , 2013, IEEE Transactions on Automatic Control.

[2]  Robert E. Mahony,et al.  Implementation of a Nonlinear Attitude Estimator for Aerial Robotic Vehicles , 2014, IEEE Transactions on Control Systems Technology.

[3]  Amirhossein Taghvaei,et al.  Error estimates for the kernel gain function approximation in the feedback particle filter , 2016, 2017 American Control Conference (ACC).

[4]  Audrey Giremus,et al.  Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions , 2014, Journal of Mathematical Imaging and Vision.

[5]  Robert Babuska,et al.  Estimation of the soil-dependent time-varying parameters of the hopper sedimentation model: The FPF versus the BPF , 2014 .

[6]  S. Aachen Stochastic Differential Equations An Introduction With Applications , 2016 .

[7]  Axel Barrau,et al.  Invariant particle filtering with application to localization , 2014, 53rd IEEE Conference on Decision and Control.

[8]  Jonathan H. Manton,et al.  A Primer on Stochastic Differential Geometry for Signal Processing , 2013, IEEE Journal of Selected Topics in Signal Processing.

[9]  François Goulette,et al.  Invariant EKF Design for Scan Matching-Aided Localization , 2015, IEEE Transactions on Control Systems Technology.

[10]  Goran Marjanovic,et al.  Numerical methods for stochastic differential equations in the Stiefel manifold made simple , 2016, 2016 IEEE 55th Conference on Decision and Control (CDC).

[11]  Robert E. Mahony,et al.  Gradient-Like Observers for Invariant Dynamics on a Lie Group , 2008, IEEE Transactions on Automatic Control.

[12]  Rita Cunha,et al.  A nonlinear position and attitude observer on SE(3) using landmark measurements , 2010, Syst. Control. Lett..

[13]  Anuj Srivastava,et al.  Bayesian and geometric subspace tracking , 2004, Advances in Applied Probability.

[14]  Victor Solo,et al.  On nonlinear state estimation in a Riemannian manifold , 2009, Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference.

[15]  M. Ledoux,et al.  Analysis and Geometry of Markov Diffusion Operators , 2013 .

[16]  Tyrone E. Duncan Some Filtering Results in Riemann Manifolds , 1977, Inf. Control..

[17]  Amirhossein Taghvaei,et al.  Attitude estimation with feedback particle filter , 2016, 2016 IEEE 55th Conference on Decision and Control (CDC).

[18]  Ulrike von Luxburg,et al.  Graph Laplacians and their Convergence on Random Neighborhood Graphs , 2006, J. Mach. Learn. Res..

[19]  B. Øksendal Stochastic differential equations : an introduction with applications , 1987 .

[20]  Jean-Philippe Condomines,et al.  Nonlinear state estimation using an invariant unscented Kalman filter , 2013 .

[21]  Amit K. Sanyal,et al.  Rigid body attitude estimation based on the Lagrange-d'Alembert principle , 2014, Autom..

[22]  Philippe Martin,et al.  Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem , 2009, Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference.

[23]  Gregory S. Chirikjian,et al.  Error propagation on the Euclidean group with applications to manipulator kinematics , 2006, IEEE Transactions on Robotics.

[24]  Audrey Giremus,et al.  Discrete Extended Kalman Filter on Lie groups , 2013, 21st European Signal Processing Conference (EUSIPCO 2013).

[25]  F. Markley,et al.  Unscented Filtering for Spacecraft Attitude Estimation , 2003 .

[26]  Jie Xiong,et al.  An Introduction to Stochastic Filtering Theory , 2008 .

[27]  G. Kallianpur Stochastic differential equations and diffusion processes , 1981 .

[28]  Robert E. Mahony,et al.  Minimum-Energy Filtering for Attitude Estimation , 2013, IEEE Transactions on Automatic Control.

[29]  Henrik I. Christensen,et al.  Robust 3D visual tracking using particle filtering on the special Euclidean group: A combined approach of keypoint and edge features , 2011, 2011 IEEE International Conference on Robotics and Automation.

[30]  Tao Yang,et al.  Feedback particle filter and its applications , 2014 .

[31]  Amirhossein Taghvaei,et al.  Feedback particle filter on matrix lie groups , 2015, 2016 American Control Conference (ACC).

[32]  Karl Berntorp,et al.  Data-driven gain computation in the feedback particle filter , 2016, 2016 American Control Conference (ACC).

[33]  Victor Solo,et al.  Geometric Euler-Maruyama Schemes for Stochastic Differential Equations in SO(n) and SE(n) , 2016, SIAM J. Numer. Anal..

[34]  Goran Marjanovic,et al.  A simple approach to numerical methods for stochastic differential equations in Lie groups , 2015, 2015 54th IEEE Conference on Decision and Control (CDC).

[35]  James Richard Forbes,et al.  Continuous-time norm-constrained Kalman filtering , 2014, Autom..

[36]  Dimitrios G. Kottas,et al.  Camera-IMU-based localization: Observability analysis and consistency improvement , 2014, Int. J. Robotics Res..

[37]  John L. Crassidis,et al.  Survey of nonlinear attitude estimation methods , 2007 .

[38]  E. J. Lefferts,et al.  Kalman Filtering for Spacecraft Attitude Estimation , 1982 .

[39]  Søren Hauberg,et al.  Unscented Kalman Filtering on Riemannian Manifolds , 2013, Journal of Mathematical Imaging and Vision.

[40]  Y. Oshman,et al.  Attitude Estimation from Vector Observations Using a Genetic-Algorithm-Embedded Quaternion Particle Filter , 2006 .

[41]  M. Pittelkau Rotation Vector in Attitude Estimation , 2003 .

[42]  Kevin C. Wolfe,et al.  Bayesian Fusion on Lie Groups , 2011 .

[43]  Karl Berntorp,et al.  Feedback particle filter: Application and evaluation , 2015, 2015 18th International Conference on Information Fusion (Fusion).

[44]  Goran Marjanovic,et al.  An engineer's guide to particle filtering on matrix Lie groups , 2016, 2016 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP).

[45]  H. Snoussi Particle Filtering on Riemannian Manifolds. Application to Covariance Matrices Tracking , 2013 .

[46]  Gregory S. Chirikjian,et al.  Gaussian approximation of non-linear measurement models on Lie groups , 2014, 53rd IEEE Conference on Decision and Control.

[47]  Frank Chongwoo Park,et al.  Particle Filtering on the Euclidean Group , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[48]  Stéphane Lafon,et al.  Diffusion maps , 2006 .

[49]  F. Tompkins,et al.  Bayesian Filtering on the Stiefel Manifold , 2007, 2007 2nd IEEE International Workshop on Computational Advances in Multi-Sensor Adaptive Processing.

[50]  Mohammad Zamani,et al.  Deterministic Attitude and Pose Filtering, an Embedded Lie Groups Approach , 2013 .

[51]  G. Chirikjian Stochastic Models, Information Theory, and Lie Groups, Volume 1 , 2009 .

[52]  B. Hall Lie Groups, Lie Algebras, and Representations: An Elementary Introduction , 2004 .

[53]  Robert E. Mahony,et al.  Gradient-like observer design on the Special Euclidean group SE(3) with system outputs on the real projective space , 2015, 2015 54th IEEE Conference on Decision and Control (CDC).

[54]  Frank Chongwoo Park,et al.  A Geometric Particle Filter for Template-Based Visual Tracking , 2014, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[55]  G. Chirikjian Stochastic Models, Information Theory, and Lie Groups, Volume 2 , 2012 .

[56]  L. Rogers Stochastic differential equations and diffusion processes: Nobuyuki Ikeda and Shinzo Watanabe North-Holland, Amsterdam, 1981, xiv + 464 pages, Dfl.175.00 , 1982 .

[57]  Philippe Martin,et al.  Non-Linear Symmetry-Preserving Observers on Lie Groups , 2007, IEEE Transactions on Automatic Control.

[58]  Rui Hirokawa,et al.  Suboptimal Gain Functions of Feedback Particle Filter Derived from Continuation Method , 2016 .

[59]  Yang Cheng,et al.  Particle Filtering for Attitude Estimation Using a Minimal Local-Error Representation , 2009 .

[60]  I. Bar-Itzhack Recursive attitude determination from vector observations Euler angle estimation , 1985 .

[61]  P. Caines,et al.  Nonlinear Filtering in Riemannian Manifolds , 1984 .

[62]  N. Trawny,et al.  Indirect Kalman Filter for 3 D Attitude Estimation , 2005 .

[63]  I. Bar-Itzhack,et al.  Attitude Determination from Vector Observations: Quaternion Estimation , 1985, IEEE Transactions on Aerospace and Electronic Systems.

[64]  F. Markley Attitude Error Representations for Kalman Filtering , 2003 .

[65]  Elton P. Hsu Stochastic analysis on manifolds , 2002 .

[66]  Sean P. Meyn,et al.  Multivariable feedback particle filter , 2016, Autom..

[67]  Y. Oshman,et al.  Averaging Quaternions , 2007 .

[68]  Robert E. Mahony,et al.  Nonlinear Complementary Filters on the Special Orthogonal Group , 2008, IEEE Transactions on Automatic Control.

[69]  A. Grigor’yan Heat Kernel and Analysis on Manifolds , 2012 .

[70]  Daniele Mortari,et al.  Norm-Constrained Kalman Filtering , 2009 .

[71]  Sean P. Meyn,et al.  Learning techniques for feedback particle filter design , 2016, 2016 IEEE 55th Conference on Decision and Control (CDC).

[72]  Axel Barrau,et al.  Intrinsic Filtering on Lie Groups With Applications to Attitude Estimation , 2013, IEEE Transactions on Automatic Control.

[73]  Stefano Soatto,et al.  Monte Carlo filtering on Lie groups , 2000, Proceedings of the 39th IEEE Conference on Decision and Control (Cat. No.00CH37187).