1 Feedback Particle Filter on Matrix Lie Groups Chi Zhang, Amirhossein Taghvaei and Prashant G. Mehta Abstract This paper is concerned with the problem of continuous-time nonlinear filtering for stochastic processesonaconnectedmatrixLiegroup.Themaincontributionofthispaperistoderivethefeedback particle filter (FPF) algorithm for this problem. In its general form, the FPF is shown to provide a 7 coordinate-free description of the filter that automatically satisfies the geometric constraints of the 1 manifold. The particle dynamics are encapsulated in a Stratonovich stochastic differential equation that 0 2 retains the feedback structure of the original (Euclidean) FPF. The implementation of the filter requires n a solution of a Poisson equation on the Lie group, and two numerical algorithms are described for this a J purpose. As an example, the FPF is applied to the problem of attitude estimation–a nonlinear filtering 0 problem on the Lie group SO(3). The formulae of the filter are described using both the rotation matrix 1 and the quaternion coordinates. Comparisons are also provided between the FPF and some popular ] algorithms for attitude estimation, namely the multiplicative EKF, the unscented quaternion estimator, C O the left invariant EKF, and the invariant ensemble Kalman filter. Numerical simulations are presented . to illustrate the comparisons. h t a m I. INTRODUCTION [ There is a growing interest in the nonlinear filtering community to develop geometric ap- 1 v proaches for handling constrained systems. In many cases, the constraints are described by 6 1 smooth Riemannian manifolds, in particular the matrix Lie groups. Engineering applications 4 2 of filtering on matrix Lie groups include: i) attitude estimation of aircrafts [29], [6]; ii) visual 0 . tracking of humans and objects [33], [17]; and iii) localization of mobile robots [4], [27]. In 1 0 these applications, the matrix Lie groups of interest include the special orthogonal group SO(3) 7 1 and the special Euclidean group SE(3). : v i X A. Problem Statement r a We consider the following continuous-time system evolving on a matrix Lie group G, dX =X V (X )dt+X V (X )◦dBα, (1a) t t 0 t t α t t dZ =h(X )dt+dW, (1b) t t t C. Zhang, A. Taghvaei and P. G. Mehta are with the Coordinated Science Laboratory and the Department of Mechanical Science and Engineering at the University of Illinois at Urbana-Champaign (UIUC) [email protected]; [email protected]; [email protected] Financial support from the NSF CMMI grants 1334987 and 1462773 is gratefully acknowledged. The conference versions of this paper appear in [61], [62]. January11,2017 DRAFT 2 where X ∈G is the state at time t, Z ∈Rm is the observation vector;V :G→G andV :G→G t t 0 α forα =1,...,r areelementsoftheLiealgebra,denotedasG;Bα andW aremutuallyindependent t t standardWienerprocessesinRandRm,respectively,andtheyarealsoassumedtobeindependent of the initial state X ; h:G→Rm is a given vector-valued nonlinear function. The j-th coordinate 0 of Z and h are denoted as Zj and h , respectively (i.e. Z =(Z1,...,Zm) and h=(h ,...,h )). t t j t t t 1 m The ◦ before dBα indicates that the stochastic differential equation (sde) (1a) is expressed in its t Stratonovich form, which provides a coordinate-free description of the sde [28]. The Einstein summation convention for the index α is used in (1a). A brief overview of matrix Lie groups and related notation is contained in Sec. II. The problem is to numerically approximate the conditional distribution of X given the time- t history of observations Z =σ(Z :s≤t). The conditional distribution, denoted as π∗, acts on t s t a function f :G→R according to π∗(f):=E[f(X )|Z ], t t t whose time-evolution is described by the Kushner-Stratonovich filtering equation (see Theorem 5.7 in [54]), (cid:90) t (cid:90) t π∗(f)= π∗(f)+ π∗(L∗f)ds+ (cid:0)π∗(fh)−π∗(h)π∗(f)(cid:1)T(cid:0)dZ −π∗(h)ds(cid:1), (2) t 0 s s s s s s 0 0 for all f ∈C∞(G) (smooth functions with compact support), where L∗f :=V · f +1∑r V · c 0 2 α=1 α (V · f). The operations V · f and V · f is defined in Sec. II. α 0 α B. Literature Review Filtering of stochastic processes in non-Euclidean spaces has a rich history; c.f., [22], [42]. In recent years, the focus has been on computational approaches to numerically approximate the conditional distribution. Such approaches have been developed, e.g., by extending the classical extended Kalman filter (EKF) to Lie groups, and the extensions have appeared in discrete- time [12], [60], continuous-time [10], [23], and continuous-discrete-time settings [6], [11]. In particular, a number of EKF-based filters have been proposed and applied for attitude estimation, e.g., the additive EKF [3] and the multiplicative EKF [36]. The EKF-based attitude filters require a linearized model of the estimation error, typically derived using one of the many three-dimensional attitude representations, e.g. the Euler angle [2], the rotation vector [45], and the modified Rodrigues parameter [39]. Apart from the EKF, particle filters for matrix Lie groups has been an active area of re- search [16], [32], [38]. Typically, particle filters adopt discrete-time description of the dynamics and are based on importance sampling and resampling numerical procedures. For the attitude estimation problem, the unscented quaternion estimator [20] and the bootstrap particle filter January11,2017 DRAFT 3 [13], [44] have been developed, using one of the attitude representations. Other non-parametric approaches include filters based on certain variational formulations on the Lie groups [59], [31]. More recently, geometric group-theoretic methods for Lie groups have been developed. De- terministic nonlinear observers that respect the intrinsic geometry of the Lie groups appear in [37], [35], [50]. A class of symmetry-preserving observers have been proposed to exploit certain invariance properties [9], leading to the invariant EKF [10], [6], the invariant unscented Kalman filter [19], the invariant ensemble Kalman filter [6], and the invariant particle filter [5] algorithms within the stochastic filtering framework. A closely related theme is the use of non-commutative harmonic analysis for characterizing error propagation and Bayesian fusion on Lie groups [15], [51], [53]. More comprehensive surveys can be found in [21], [58]. C. Overview of the Paper The objective of this paper is to obtain a generalization of the feedback particle filter (FPF), originally developed in [55], [56], [57] in the Euclidean settings, to the filtering problem (1a)- (1b). The main result is to show that the update formula in the original Euclidean setting carries over to the manifold setting. The contributions of this paper are as follows: •FeedbackparticlefilteronmatrixLiegroups.TheextensionoftheFPFformatrixLiegroups is derived. The particle dynamics, expressed in their Stratonovich form, are shown to provide a coordinate-free description of the filter that automatically satisfies the geometric constraints of the manifold. Even in the manifold setting, the FPF is i) shown to admit an error-correction gain-feedback structure, and ii) proved to be an exact algorithm. Exactness means that, in the limit of large number of particles, the empirical distribution of the particles exactly matches the posterior distribution. • Poisson equation on matrix Lie groups. The FPF algorithm requires numerical approximation of the gain function as a solution to a linear Poisson equation on the Lie group. An existence- uniqueness result for the solution is described in the Lie group setting. Two numerical methods are proposed to approximate the solution: i) In the Galerkin scheme, the gain function is approximated using a set of pre-defined basis functions; ii) In the kernel-based scheme, a numerical solution is obtained by solving a certain fixed-point equation. • Feedback particle filter for attitude estimation. The attitude estimation problem represents thespecialcasewheretheLiegroupisSO(3).Forthisimportantspecialcase,theexplicitformof the filter is described with respect to both the rotation matrix and the quaternion coordinates, with the latter being demonstrated for computational purposes. The FPF is also compared with some popular attitude filters, including the multiplicative EKF, the unscented quaternion estimator, the January11,2017 DRAFT 4 left invariant EKF, and the invariant ensemble Kalman filter. Simulation studies are presented to illustrate the performance comparison of these filters and the FPF algorithm. • The FPF with concentrated distributions. The primary challenge in implementing the FPF algorithm arises due to the gain function approximation. In a certain special case, namely where the posterior distribution is concentrated, certain closed-form approximation, referred to as the constant gain approximation, of the gain function is obtained. For this approximation, evolution equations for the mean and the covariance are also derived and shown to be closely related to the left invariance EKF algorithm. D. Organization of the Paper The outline of the remainder of this paper is as follows: A brief review of the relevant Lie group preliminaries is contained in Sec. II. In Sec. III, the generalization of the FPF algorithm to matrix Lie groups is presented, including both theory and numerical algorithms. The FPF algorithm for attitude estimation and its special case with concentrated conditional distributions are described in Sec. IV and Sec. V, respectively. Numerical simulations are provided in Sec. VI. The proofs appear in the Appendix. II. MATHEMATICAL PRELIMINARIES This section includes a brief review of matrix Lie groups. The intent is to fix the notation used in subsequent sections. A. Geometry of Matrix Lie Groups The general linear group, denoted as GL(n;R), is the group of n×n invertible matrices, where the group operations are the matrix multiplication and inversion. The identity element is the identity matrix, denoted as I. A matrix Lie group, denoted as G, is a closed subgroup of GL(n;R). G is assumed to be connected. The Lie algebra of G, denoted as G, is the set of matrices V such that the matrix exponential, exp(tV), is in G for all t ∈ R. G is a vector space whose dimension, denoted as d, equals the dimension of the group. The Lie algebra is equipped with an inner product, denoted as (cid:104)·,·(cid:105)G, and an orthonormal basis {E1,...,Ed} with (cid:112) (cid:104)Ei,Ej(cid:105)G =δij. The norm of V ∈G is defined as |V|G := (cid:104)V,V(cid:105)G. Example: The special orthogonal group SO(3) is the group of 3×3 matrices R such that RRT = I and det(R) = 1. The Lie algebra so(3) is the 3-dimensional vector space of skew- symmetric matrices. An inner product is (cid:104)Ω ,Ω (cid:105) = 1 Tr(ΩTΩ ) for Ω ,Ω ∈so(3), and an 1 2 so(3) 2 1 2 1 2 orthonormal basis {E ,E ,E } of so(3) is given by, 1 2 3 0 0 0 0 0 1 0 −1 0 E =0 0 −1, E = 0 0 0, E =1 0 0. (3) 1 2 3 0 1 0 −1 0 0 0 0 0 January11,2017 DRAFT 5 Fig. 1: Construction of vector-fields on G. These matrices have the physical interpretation of generating rotations about the three canonical axes in R3. Here, det(·) and Tr(·) denote the determinant and trace of a matrix, respectively. Given the basis in (3), a vector ω = (ω ,ω ,ω ) ∈ R3 is uniquely mapped to an element in 1 2 3 so(3), denoted as Ω=[ω] :=ω E +ω E +ω E . Conversely, ω :=[Ω]∨. × 1 1 2 2 3 3 TheLiealgebraisidentifiedwiththetangentspaceattheidentitymatrixI,andcanfurthermore be used to construct a basis {Ex,...,Ex} for the tangent space at x ∈ G, where Ex = xE for 1 d n n n=1,...,d. Therefore, a vector-field on G, denoted as V , is expressed as V (x)=v (x)Ex+···+v (x)Ex, 1 1 d d with v (x) : G → R for n = 1,...,d. We write V (x) = xV(x), where V(x) := v (x)E +···+ n 1 1 v (x)E is an element of the Lie algebra G for each x ∈ G. The functions (cid:0)v (x),...,v (x)(cid:1) d d 1 d are referred to as the coordinates of the vector-field. The construction of vector-fields on G is illustrated in Figure 1. The inner product (cid:104)·,·(cid:105)G in the Lie algebra induces an inner product of two vector-fields, d (cid:104)V ,W (cid:105)(x):=(cid:104)V(x),W(x)(cid:105)G = ∑ vn(x)wn(x), n=1 (cid:112) along with the associated norm |V | (x):= (cid:104)V ,V (cid:105)(x). G With a slight abuse of notation, the action of the vector-field V on a function f :G→R is denoted as, d (cid:12) (cid:0) (cid:1) V · f(x):= (cid:12) f xexp(τV(x)) . (4) (cid:12) dτ τ=0 We also define the vector-field, grad(φ), for a function φ :G→R as, grad(φ)(x):=xK(x), (5) where K(x) ∈ G with coordinates (cid:0)E ·φ(x),...,E ·φ(x)(cid:1) and according to (4), (E ·φ)(x) := 1 d n ddτ(cid:12)(cid:12)τ=0φ(cid:0)xexp(τEn)(cid:1) for n=1,...,d. The vector-field acts on a function f as, d K· f(x)=(cid:104)grad(φ),grad(f)(cid:105)(x)= ∑ E ·φ(x)E · f(x). (6) n n n=1 January11,2017 DRAFT 6 We consider the following function spaces: The vector space of smooth real-valued functions f : G → R with compact support is denoted as C∞(G). For a probability distribution π on G, c L2(G;π) denotes the Hilbert space of functions on G that satisfy π(|f|2)<∞ (here π(|f|2):= (cid:82) |f|2dπ(x)); H1(G;π) denotes the Hilbert space of functions f such that f and E · f (defined G n in the weak sense) are all in L2(G;π); and H1(G;π):={φ ∈H1(G;π)|π(φ)=0}. 0 B. Quaternions Quaternions provide a computationally efficient coordinate representation for SO(3). A unit quaternion has the form, (cid:16) θ θ θ θ (cid:17) q=(q , q , q , q )= cos( ), ω sin( ), ω sin( ), ω sin( ) , (7) 0 1 2 3 1 2 3 2 2 2 2 which represents rotation of angle θ about the axis defined by the unit vector (ω ,ω ,ω ). As 1 2 3 with SO(3), the space of quaternions admits a Lie group structure: The identity quaternion is denoted as q :=(1,0,0,0), the inverse of q is denoted as q−1 :=(q ,−q ,−q ,−q ), and the I 0 1 2 3 multiplication is defined as, (cid:34) (cid:35) p q −p ·q 0 0 V V p⊗q:= , p q +q p +p ×q 0 V 0 V V V where p = (p ,p ,p ), q = (q ,q ,q ), and · and × denote the dot product and the cross V 1 2 3 V 1 2 3 product between two vectors. Given a unit quaternion q, the corresponding rotation matrix R∈SO(3) is calculated by, 2q2+2q2−1 2(q q −q q ) 2(q q +q q ) 0 1 1 2 0 3 1 3 0 2 R=2(q q +q q ) 2q2+2q2−1 2(q q −q q ). (8) 1 2 0 3 0 2 2 3 0 1 2(q q −q q ) 2(q q +q q ) 2q2+2q2−1 1 3 0 2 2 3 0 1 0 3 For more comprehensive introduction of matrix Lie groups and quaternions, we refer the reader to [25], [49]. III. FEEDBACK PARTICLE FILTER ON MATRIX LIE GROUPS This section extends the FPF algorithm originally proposed in [56] to matrix Lie groups, with necessary modifications to the original framework to account for the manifold structure. A. Particle Dynamics and Control Architecture The FPF on a matrix Lie group G is a controlled system comprising of N stochastic processes {Xi}N with Xi ∈G. The particles are modeled by the Stratonovich sde, t i=1 t dXi = Xi(cid:0)V (Xi)+u(Xi,t)(cid:1)dt+XiV (Xi)◦dBα,i+XiK (Xi,t)◦dZj, (9) t t 0 t t t α t t t j t t January11,2017 DRAFT 7 α,i where B for α =1,...,r and i=1,...,N are mutually independent standard Wiener processes t in R, and the Einstein summation convention is used for the indices α and j. The functions u(x,t), K (x,t) : G×[0,T] → G are referred to as the control and gain function, respectively, j whose coordinates are denoted as (u ,...,u ) and (k ,...,k ), for j=1,...,m. These functions 1 d 1,j d,j need to be chosen. The following admissibility requirement is imposed on u and K : j Definition 1: (Admissible Input): The functions u(x,t) and K (x,t) are admissible if they are j Zt−measurable and E[(cid:0)∑n|un(Xti,t)|(cid:1)]<∞, E[∑n|kn,j(Xti,t)|2]<∞ for each j=1,...,m and for all t. The conditional distribution of the particle Xi given Z is denoted by π , which acts on a t t t function f according to π (f):=E[f(Xi)|Z ]. t t t The evolution equation for π is given by the proposition below. The proof appears in Appendix t A. Proposition 1: Consider the particle Xi with dynamics described by (9). The forward evolution t equation of the conditional distribution π is given by, t (cid:90) t (cid:90) t π (f)=π (f)+ π (L f)ds+ π (K · f)dZj, (10) t 0 s s j s 0 0 for any f ∈C∞(G), where the operator L is defined as c 1 r 1 m L f :=(V +u)· f + ∑ V ·(V · f)+ ∑K ·(K · f). 0 α α j j 2 2 α=1 j=1 Problem statement: There are two types of conditional distributions of interest: • πt∗: The conditional distribution of Xt given Zt. • πt: The conditional distribution of Xti given Zt. The functions u(x,t),K (x,t) are said to be exact if π =π∗ for all t ∈[0,T]. Thus, the objective j t t is to choose u and K such that, given π =π∗, the evolution of the two conditional distributions j 0 0 are identical (see (2) and (10)). Solution:TheFPFrepresentsthefollowingchoiceofthegainfunctionKandthecontrolfunction u: 1) Gain function: The gain function is obtained as follows: For j =1,...,m, let φ ∈H1(G;π ) j t be the solution of a linear Poisson equation: π (cid:0)(cid:104)grad(φ ),grad(ψ)(cid:105)(cid:1)=π (cid:0)(h −hˆ )ψ(cid:1), t j t j j (11) π (φ )=0 (normalization), t j January11,2017 DRAFT 8 for all ψ ∈H1(G;π ), where hˆ :=π (h ). The gain function K ∈G is then chosen as, t j t j j xK (x,t)=grad(φ )(x). (12) j j Given a basis {E }d of the Lie algebra G, and noting that (see (5)) n n=1 grad(φ )(x)=E ·φ (x)Ex+···+E ·φ (x)Ex j 1 j 1 d j d where Ex =xE , the coordinates of K is given by n n j k (x,t)=E ·φ (x) , for n=1,...,d. (13) n,j n j 2) Control function: The function u is chosen as, u(x,t)=−1 ∑m K (x,t)(cid:0)h (x)+hˆ (cid:1). (14) j j j 2 j=1 Feedback particle filter: Using these choice of u and K, the i-th particle in the FPF has the following representation: dXi =XiV (Xi)dt+XiV ◦dBα,i+Xi K (Xi,t)◦dIj,i, (15) t t 0 t t α t t j t t (cid:124) (cid:123)(cid:122) (cid:125) (cid:124) (cid:123)(cid:122) (cid:125) propagation observation update where the error dIj,i ∈R is a modified form of the innovation process: t h (Xi)+hˆ dIj,i = dZj− j t j dt, (16) t t 2 for each j=1,...,m. The i-th particle implements the Bayesian update step–to account for the conditioning due to the observations–as gain times an error, which is akin to the feedback structure in a classical Kalman filter. Note that the Poisson equation (11) must be solved for each j=1,...,m, and for each time t. The exactness is asserted in the following theorem. The proof is contained in appendix B. Theorem 1: Let π∗ and π satisfy the forward evolution equations (2) and (10), respectively. t t Suppose that the gain functions K , j =1,...,m, are obtained using (11)-(12), and the control j function u is obtained using (14). Suppose also that these functions are admissible. Then, assuming π =π∗, we have, 0 0 π (f)=π∗(f), t t for all t ∈[0,T] and all function f ∈C∞(G). c Remark 1: In the original Euclidean setting, the FPF has the prettiest–gain times error– representation of the update step in the Stratonovich form of the filter (see Remark 1 in [56]). In the Itoˆ form, the filter includes an additional Wong-Zakai correction term. For sdes on a manifold, it is well known that the Stratonovich form is invariant to coordinate transformations while the Itoˆ form is not [43]. So, for the gain times error form of the update step to have an January11,2017 DRAFT 9 intrinsic coordinate independent form, the multiplication must necessarily be in the Stratonovich form. The upshot is that the gain and error formula for the update step in (15) is intrinsic to the filter. Remark 2: The equation (11) is the weak form of a Poisson equation. Suppose π admits an t everywhere positive density, denoted as ρ . Then the strong form of (11) is given by the standard t Poisson equation, ∆ φ =−(h −hˆ ), (17) ρt j j where ∆ φ := 1 div(cid:0)ρ grad(φ)(cid:1) is the weighted Laplacian on the manifold [24], and div(·) ρt ρt t denotes the divergence operator. Multiplying both sides of (17) by ψ(x)ρ (x) and integrating by t parts, one arrives at the weak form (11). In the Euclidean case, the gain function was obtained as the gradient of the solution of a Poisson equation [56]. Remark 2 shows that the Euclidean gain function is a special case of the more general Lie group formula (11)-(12). For the latter, the definition of divergence and gradientensuresthatthePoissonequationhasacoordinate-freerepresentation.Thegainfunction, expressed as gradient of the solution of the Poisson equation, is an element of the Lie algebra. This is consistent with the use of Lie algebra to define the vector fields for dynamics evolving on the Lie group. In summary, the FPF is an intrinsic algorithm. The FPF update formula not only provides for a generalization of the Kalman filter to the nonlinear non-Gaussian case but also that the generalization carries over to nonlinear spaces such as the Lie groups. This is expected to have important consequences for many applications in vision and robotics where Lie groups naturally arise. B. Well-posedness and Admissibility of the Gain Theadmissibilityofthegainfunctionsolution,i.e.,E[(cid:0)∑n|un(Xti,t)|(cid:1)]<∞andE[∑n|kn,j(Xti,t)|2]< ∞, requires a well-posedness analysis of the Poisson equation. As in the original Euclidean setting, we make the following assumptions: Assumption 1: The function h ∈L2(G;π ) for each j=1,...,m and for all t. j t Assumption 2: The distribution π admits a uniform spectral gap (or Poincare´ inequality) with t constant λ¯ (Sec. 4.2 in [1]): That is, for a function φ ∈H1(G;π ) and for all t ∈[0,T], 0 t π (cid:0)|φ|2(cid:1)≤ 1 π (cid:0)|grad(φ)|2(cid:1). t ¯ t λ The proof of the following well-posedness theorem is identical to the proof presented in [56] for the Euclidean case. It is omitted. January11,2017 DRAFT 10 Theorem 2: Under Assumption 1 and Assumption 2, the Poisson equation (11) possesses a unique solution φ ∈H1(G;π ), satisfying j 0 t π (cid:0)|grad(φ )|2(cid:1)≤ 1 π (cid:0)|h −hˆ |2(cid:1), t j G ¯ t j j λ for each j=1,...,m and for all t. For this solution, one has the following bounds, π (cid:0)|K |2 (cid:1)≤ 1 π (cid:0)|h −hˆ |2(cid:1), j=1,...,m, t j G ¯ t j j λ d m π (cid:0) ∑ |u |(cid:1)≤C ∑π (cid:0)|h |2(cid:1), t n t j n=1 j=1 ¯ where the constant C depends on λ. That is, the resulting gain and control functions are admissible according to Definition 1. Remark 3 (Remark on Assumptions A1-A2): Suppose the Lie group G is compact, e.g., SO(3). In this case, if π has an everywhere positive density ρ , then Assumption 1 and 2 automatically t t hold. For non compact manifolds, e.g., SE(3), the assumptions hold if the density ρ has a t Gaussian tail (see Remark 2 in [56]). The main challenge to implement the FPF algorithm is to approximate the gain function solution. Since the problem (11) is linear, the approximation involves constructing a matrix problemtoobtaintheapproximatesolution.Inthefollowingtwosections,twonumericalschemes for the approximation are presented. Since the equations for each j = 1,...,m are uncoupled, without loss of generality, a scalar-valued observation is assumed (i.e., m=1, and φ , h , K are j j j denoted as φ, h, K). As the time t is fixed, the explicit dependence on t is suppressed (i.e., π , t Xi are denoted as π, Xi). t C. Galerkin Gain Function Approximation In a Galerkin approach, the solution φ is approximated as, L φ = ∑κ ψ , l l l=1 where {ψ }L is a given (assumed) set of basis functions on G. The coordinates of the gain l l=1 function K with respect to a basis {E }d of G are then given by (see (13)), n n=1 L k = ∑κ E ·ψ , n=1,...,d. n l n l l=1 The finite-dimensional approximation of the Poisson equation (11) is to choose coefficients {κ }L such that, l l=1 L ∑κ π(cid:0)(cid:104)grad(ψ ),grad(ψ)(cid:105)(cid:1)=π(cid:0)(h−hˆ)ψ(cid:1), (18) l l l=1 January11,2017 DRAFT