Bivariate Angular Estimation Under Consideration of Dependencies Using Directional

Gerhard Kurz1, Igor Gilitschenski1, Maxim Dolgov1 and Uwe D. Hanebeck1

Abstract— Estimation of angular quantities is a widespread issue, but standard approaches neglect the true topology of the problem and approximate directional with linear uncertainties. In recent years, novel approaches based on directional statistics have been proposed. However, these approaches have been unable to consider arbitrary circular correlations between multiple angles so far. For this reason, we propose a novel recursive filtering scheme that is capable of estimating multiple angles even if they are dependent, while correctly describing their circular correlation. The proposed approach is based on toroidal probability distributions and a circular correlation coefficient. We demonstrate the superiority to a standard approach based on the Kalman filter in simulations. Fig. 1: A bivariate wrapped normal Index Terms— recursive filtering, wrapped normal, circular on the torus shown as a heat map. correlation coefficient, moment matching.

I.INTRODUCTION our knowledge, this is the first work on recursive estimation There are many applications that require estimation of based on toroidal probability distributions. angular quantities. These applications include, but are not It should be noted that there are some directional filters that, limited to, robotics, augmented reality, and aviation, as well in a sense, take correlation between angles into account. We as biology, geology, and medicine. In many cases, not just have performed research on a filter based on the hyperspheri- one, but several angles have to be estimated. Furthermore, cal [8], [9], which captures correlations correlations may exist between those angles and have to be when estimating rotations represented as . A very taken into account in the estimation algorithm. For example, similar approach has independently been published by Glover there may be dependencies between the orientations of head et al. [10]. Furthermore, Feiten et al. have used mixtures of and torso of a person. Another example is a robot arm with projected Gaussians to deal with 6D pose estimation while several rotary joints that are affected by correlated noise. considering correlations between different angles as well as Traditional approaches for estimating correlated angles are correlation between position and orientation [11]. However, all typically based on Gaussian distributions and use classical of these approaches are intended for describing 3D rotations, filtering algorithms such as the Kalman filter [1] or nonlinear which have a different underlying topology, namely the group extensions thereof, e.g., the unscented Kalman filter [2]. SO(3) [12], rather than the torus. For this reason, they cannot However, Gaussian distributions are defined on n-dimensional be used to estimate arbitrary correlated angles. vector spaces rather than the proper manifold, in this case a In the field of directional statistics, some previous work on torus or hypertorus. toroidal distributions can be found. In particular, multivariate Some filtering algorithms that are particularly well-suited generalizations of the have been for angular estimation have been proposed. They rely on studied by several authors [13], [14]. The multivariate periodic probability distributions that stem from the field wrapped has also been considered [4, of directional statistics [3], [4]. For example, Azmani et Sec. 2.3.2], [15], which will be the foundation of the algorithm al. proposed a filtering algorithm based on the von Mises that we propose in this paper. Furthermore, various directional distribution [5], [6]. In our prior work, we proposed an versions of the correlation coefficient have been suggested. algorithm based on the wrapped normal distribution [7]. We use the circular correlation coefficient as defined in [4, However, these approaches are one-dimensional and unable Sec. 8.2] and [15]. to take correlations between several angles into account. To address this deficiency, we propose a new filtering II.TOROIDAL STATISTICS algorithm for estimation of correlated angles in this paper. To In this section, we give an introduction to toroidal statistics. 1 The authors are with the Intelligent Sensor-Actuator-Systems Laboratory First of all, we define the necessary topological spaces. The (ISAS), Institute for Anthropomatics and Robotics, Karlsruhe Institute of Technology (KIT), Germany (e-mail: [email protected], unit circle [email protected], [email protected], 1 [email protected]). S ={x ∈ C : ||x|| = 1} ={cos(φ) + i sin(φ) : 0 ≤ φ < 2π} distribution with parameters µ and Σ. Note that the parameter matrix stems from the covariance of a bivariate normal is identified with the interval S1 ≡ [0, 2π), while keeping distribution, yet its meaning is different in the toroidal context. the topology in mind. The torus T 2 = S1 × S1 is obtained An example of the TWN distribution is depicted in Fig. 2a. as the Cartesian product of two circles. More generally, the It can be seen how x and x are 2π-periodic and the n-torus 1 2 distribution wraps at these locations. T n = S1 × · · · × S1 = (S1)n | {z } n times B. Toroidal Moments is obtained by the n-fold Cartesian product of circles. We In analogy to the traditional linear moments, we introduce only consider T 2 in the remainder of this paper. Most of the the circular moments and subsequently generalize them to presented techniques can be generalized to the n-torus. toroidal moments. A. Toroidal Distributions Definition 3 (Circular Moments). Before we look at the bivariate toroidal wrapped normal In the univariate case, the n-th circular moment (sometimes distribution, we introduce the circular univariate wrapped also referred to as trigonometric or angular moment) of a normal distribution to show how the toroidal distribution is a random variable x is given by generalization of the circular case. Z 2π inx inx Definition 1 (Wrapped Normal Distribution). mn = E(e ) = f(x)e dx ∈ C , A univariate wrapped normal (WN) distribution is given by 0 its probability density function (pdf) where i is the imaginary unit. ∞ X Note that the n-th circular moment is a complex number, f(x; µ, σ) = N (x; µ + 2πj, σ) i.e., it has two degrees of freedom. The argument of m1 j=−∞ determines the location of the circular mean, whereas the 1 1 with x ∈ S , location parameter µ ∈ S , dispersion parameter absolute value of m1 determines the concentration. For this σ > 0, and normal density N (x; µ, σ). reason, a WN distribution is uniquely determined by its first circular moment. We generalize circular moments to the We use the notation X ∼ WN (µ, σ) to indicate that bivariate case in the following Definition. a random variable X is distributed according to a WN distribution with parameters µ and σ. A WN distribution Definition 4 (Toroidal Moments). is obtained by wrapping a normal distribution around the For a random variable x distributed according to a toroidal unit circle. The normalization constant is already included distribution, the n-th toroidal moment is given by in the Gaussian distributions, so it does not need to be  inx1  Z 2πZ 2π  inx1  calculated separately. This is a significant advantage compared e e 2 mn = E inx2 = f(x) inx2 dx1dx2 ∈ C . to other periodic probability distributions whose normalization e 0 0 e constants can be difficult to calculate. The WN distribution The n-th bivariate circular moment is a vector of two can be generalized to the bivariate case as follows. complex numbers, and thus, has four degrees of freedom. Definition 2 (Toroidal Wrapped Normal Distribution). Lemma 1 (Moments of a TWN distribution). The toroidal (or bivariate) wrapped normal (TWN) distribu- The n-th moment of TWN (µ, Σ) is given by tion is given by the pdf ∞ ∞     " # " 2 2 # X X 2πj mn,1 exp(inµ1 − n σ /2) f(x; µ, Σ) = N x; µ + , Σ m = = 1 , 2πk n 2 2 j=−∞ k=−∞ mn,2 exp(inµ2 − n σ2/2) T 1 1 with x = [x1, x2] ∈ S × S , location parameter µ = i.e., the componentwise circular moment of a WN (µ1, σ1) T 1 1 [µ1, µ2] ∈ S × S , and symmetric parameter matrix and a WN (µ2, σ2).  2  σ1 ρσ1σ2 2×2 The proof is given in the appendix. Note that the n-th Σ = 2 ∈ R · σ2 bivariate circular moment does not depend on the linear correlation coefficient ρ. with correlation1 parameter −1 < ρ < 1, linear standard deviations σ , σ > 0, and multivariate normal distribution 1 2 C. Circular Correlation Coefficient N (x; µ, Σ). Several circular correlation coefficients have been defined The notation X ∼ T WN (µ, Σ) is used to indicate that (for example by Mardia [16], Johnson [17], Jupp [18], and a random variable X is distributed according to a TWN Fisher [19]). We use the definition of Jammalamadaka et al. 1While the linear correlation coefficient ρ can reach values −1 and 1, [15], [4], because it is intuitive, easy to work with and has a we do not consider these cases because they lead to a positive semidefinite variety of nice properties (see [15, Theorem 2.1]). rather than a positive definite covariance matrix. 1

c 0.5 ρ

0 4 4 3 3 2 2 1 1 σ σ 2 0 0 1 (a) A bivariate wrapped normal probability distribution with (b) Circular correlation coefficient for a TWN distribution with T parameters µ = [1, 6] and Σ = [1, 0.5; 0.5, 1] Keep in mind given σ1, σ2 and ρ → 1. As can be seen, the circular correlation that x1 and x2 are 2π-periodic. coefficient ρc is significantly lower than ρ for large σ1, σ2. Fig. 2: Toroidal wrapped normal distribution and circular correlation.

Definition 5 (Circular Correlation Coefficient). match both the toroidal moments and the circular correlation The circular correlation coefficient of random variables α, β to capture all five degrees of freedom of a TWN distribution. with circular means µ, ν is defined as An algorithm for estimating TWN parameters is given (sin(α − µ) sin(β − ν)) in [15, eq. (3.4), (3.5)], which is based on calculating the E ix ix ρc(α, β) = circular moments m = (e 1 ) and m = (e 2 ) as pVar(sin(α − µ)) Var(sin(β − ν)) 1,1 E 1,2 E well as the product expectation value E(ei(x1−µ1)ei(x2−µ2)) E(sin(α − µ) sin(β − ν)) of some distribution and fitting a TWN distribution with = q ∈ [−1, 1] . 2 2 identical moments. This is not always possible, because the E(sin (α − µ))E(sin (β − ν)) resulting Σ-matrix is not necessarily positive definite and Note that this definition of the circular correlation coeffi- thus, not a valid covariance matrix. Furthermore, this method cient closely resembles the linear correlation coefficient does not maintain the circular correlation coefficient ρc. E((x − µ)(y − ν)) We propose a better alternative obtained by matching the ρ(x, y) = p circular correlation coefficient. E((x − µ)2) · E((y − ν)2) of real-valued random variables x, y with means µ, ν. The Lemma 3 (Estimation of TWN Parameters by Moment Matching). circular correlation coefficient is obtained by applying the 2 sin(·) function to the considered quantities. Similar to the For a given first moment m1 ∈ C and a given circular correlation coefficient ρc ∈ (−1, 1), there exists a TWN linear correlation coefficient, it holds that −1 ≤ ρc ≤ 1. For distribution with identical first moment m and circular independent α and β, we have ρc = 0, but the converse is 1 correlation coefficient ρc if and only if ρ ∈ (−1, 1), where not necessarily true. On the other hand, we have ρc = ±1 if and only if α = ±β + const mod 2π (assuming α and β atan2(Im m , Re m ) µ = 1,1 1,1 have full support). atan2(Im m1,2, Re m1,2) q q Lemma 2 (Correlation Coefficient of TWN Distribution). σ1 = −2 log ||m1,1|| , σ2 = −2 log ||m1,2|| The circular correlation according to Definition 5 for q  T 1 −1 2 2 [x1, x2] ∼ T WN (µ, Σ) is given by ρ = sinh sinh(σ1) sinh(σ2) · ρc . σ1σ2 sinh(σ1σ2ρ) ρc(x1, x2) = . We give the proof in the appendix. The inverse of the psinh(σ2) sinh(σ2) −1 1 2 hyperbolic√ sine can be calculated according to sinh (x) = The derivation can be found in [15]. The TWN has five log(x + x2 + 1) [20, eq. (4.6.20)]. degrees of freedom, (µ1, µ2, σ1, σ2, ρ), and it is uniquely III.OPERATIONS ON TWNDENSITIES determined by its first toroidal moment together with its In order to create a toroidal filtering algorithm, we need circular correlation coefficient. The largest possible circular to perform certain operations on TWN distributions, which correlation coefficient for a TWN distribution is depicted in we derive in this section. Fig. 2b. A. Convolution of TWN Densities D. Moment Matching The convolution of pdfs is required for prediction, as we Moment matching is a well-known technique for a variety will show in Sec. IV-A. Convolution of pdfs corresponds of problems and we can use a similar approach in the to the addition of random variables, in this case, addition directional case. For univariate problems, it is natural to modulo 2π. match the first circular moment, which captures both location and dispersion of the considered distribution (e.g., a WN Lemma 4 (Convolution of two TWN distributions). distribution) [7]. In the bivariate case, however, we need to For A ∼ T WN (µa, Σa) and B ∼ T WN (µb, Σb), we have Z 2πZ 2π A + B ∼ T WN (µ, Σ) = TWN (µa, Σa) ∗ T WN (µb, Σb), 2 = sin (x1 − µ1) a b a b where µ = µ + µ mod 2π , Σ = Σ + Σ . 0 0 · f(x; µa, Σa)f(x; µb, Σb)dx dx , Proof. This result follows immediately from the convolution 1 2 2 formula for Gaussian distributions. s22 :=E(sin (x2 − µ2)) Z 2πZ 2π 2 B. Multiplication of TWN Densities = sin (x2 − µ2) 0 0 To perform a Bayesian update, we need to be able to cal- a a b b culate the product of two TWN probability density functions. · f(x; µ , Σ )f(x; µ , Σ )dx1dx2 . Unfortunately, the resulting function is, in general, not the We calculate the values s12, s11, and s22 by numerical unnormalized pdf of a TWN, because TWN distributions are integration. Then, we apply Lemma 3 again to obtain ρ. not closed under multiplication. This is not surprising since As mentioned in Lemma 3, a solution does not necessarily not even WN distributions are closed under multiplication exist. There are different approaches to handle the cases [7]. where moment matching is not possible. For example, we 1) Problem Formulation: We seek to derive an approxima- can try to find the TWN distribution that, in some sense, is tion, i.e., we try to fit a TWN distribution to the true product closest to the true distribution even though it does not have density. We propose to use moment matching in order to the same moments. Because this problem appears very rarely obtain the TWN distribution of the product. in practice, we handle it by ignoring the measurement in these We consider two TWN densities TWN (µa, Σa) and cases. This approach is similar to the common solution of TWN (µb, Σb), and we try to obtain a TWN density ignoring measurement that would cause the covariance matrix TWN (µ, Σ), such that the moments and circular correlation to loose the property of being positive definite in standard of the renormalized true product density nonlinear filtering algorithms, such as the UKF. TWN (µa, Σa) ·TWN (µb, Σb) IV. TOROIDAL FILTERING ALGORITHM R 2π R 2π a a b b We denote the system state at time step k with x ∈ T 2 0 0 TWN (µ , Σ ) ·TWN (µ , Σ )dx1dx2 k and consider the system model and the density of TWN (µ, Σ) match. The matrix Σ is x = x + w mod 2π composed of σ1, σ2, and ρ, as given in Definition 2. k+1 k k 2) Obtaining µ and σ1, σ2: To determine µ, σ1, σ2, we with TWN distributed system noise w ∼ T WN (µw, Σw). k k k need to calculate We interpret the modulo operator componentwise, i.e., (a, b)T Z 2πZ 2π T a a b b mod 2π = (a mod 2π, b mod 2π) . The measurement c = f(x; µ , Σ )f(x; µ , Σ )dx1dx2 zˆ ∈ T 2 is disturbed by noise according to 0 0 k 1 m = (exp(ix )) zˆk = xk + vk mod 2π , 1,1 c E 1 where v is TWN distributed measurement noise with v ∼ 1 Z 2πZ 2π k k ix1 a a b b TWN (µv , Σv ). The filtering algorithm consists of prediction = e f(x; µ , Σ )f(x; µ , Σ )dx1dx2 k k c 0 0 and measurement update steps. 1 m = (exp(ix )) 1,2 c E 2 A. Prediction Step 1 Z 2πZ 2π The prediction step propagates the current estimate through ix2 a a b b = e f(x; µ , Σ )f(x; µ , Σ )dx1dx2 . time. For this purpose, we assume the system model to be c 0 0 the identity. However, we allow for non-zero-mean system These integrals are difficult to evaluate because even the noise, which allows us to model system equations which add integral over a two-dimensional Gaussian distribution can a known angle as well. only be evaluated numerically [21]. For this reason we use The predicted distribution consists of the convolution of numerical integration [22]. From m and m , we obtain 1,1 1,2 the estimated distribution and the system noise distribution. µ, σ , σ as given in Lemma 3. 1 2 This fact can be shown by calculating 3) Obtaining ρ: To calculate the circular correlation p coefficient, we need fk+1(xk+1) Z s12 e ρc = √ = f(xk+1|xk)fk (xk)dxk s11s22 T 2 Z Z where w e = f(xk+1|wk, xk)fk (wk)dwkfk (xk)dxk T 2 T 2 s12 := (sin(x1 − µ1) sin(x2 − µ2)) Z Z E w e Z 2πZ 2π = δ(xk+1 − wk − xk)fk (wk)dwkfk (xk)dxk T 2 T 2 = sin(x1 − µ1) sin(x2 − µ2) Z 0 0 = f w(x − x )f e(x )dx a a b b k k+1 k k k k · f(x; µ , Σ )f(x; µ , Σ )dx1dx2 , T 2 w e 2 =(f ∗ f )(x ) , s11 :=E(sin (x1 − µ1)) k k k+1 where ∗ denotes convolution. Thus, we can directly apply V. EVALUATION the convolution as introduced in section III-A. The resulting We evaluated the proposed approach in simulations. Unless algorithm is given in Algorithm 1. specified otherwise, all angles are given in radians. For comparison, we use a modified Kalman filter [1] with Algorithm 1: Prediction of the proposed filter. two-dimensional state vector. The unmodified Kalman filter Input: estimate TWN (µe , Σe ), system noise k k fails completely once the periodic boundary is crossed. To TWN (µw, Σw) k k perform a fair comparison, we modify the measurement Output: prediction TWN (µp , Σp ) k+1 k+1 update step of the regular Kalman filter by introducing a µp ← µe + µw mod 2π; preprocessing of the measurement. Before the application k+1 k k p e w of the Kalman filter update formulas, we reposition the Σk+1 ← Σk + Σk ; p p measurement in such a way that its distance to the mean return TWN (µ , Σk+1); k+1 of the current estimate is at most π in each dimension. The algorithm is given in Algorithm 3.

B. Measurement Update Step Algorithm 3: Measurement update for modified Kalman In order to incorporate the information from a measurement filter. into the state estimate, we perform a Bayesian measurement Input: prediction N (µp, Σp), measurement noise k k update step. In analogy to the derivation in [7], the estimated N (µv , Σv ), measurement z k k density is given by Output: estimate N (µe , Σe ) k k e p /* preprocessing */ fk (xk) = f(xk|zˆk) = c · f(ˆzk|xk)f (xk) k for n = 1, 2 do = c · f v(ˆz − x )f p(x ) , k k k k k if |(µp) − z | > π then k n n v   i.e., the renormalized product of the noise density f (ˆz −x ) p k k k zn ← zn + 2π sgn (µ )n − zn ; and the prior density f p(x ). The density f v(ˆz − x ) is k k k k k k end given by TWN (ˆz − µv , Σv ). The complete algorithm is k k k end given in Algorithm 2. /* Kalman filter update */ p p v −1 K ← Σk(Σk + Σk) ; Algorithm 2: Measurement update of the proposed filter. µe ← µp + K(z − µp) mod 2π ; k k k Input: prediction TWN (µp, Σp), measurement noise e p k k Σk ← (I2×2 − K)Σk; TWN (µv , Σv ), measurement zˆ return N (µe , Σe ); k k k k Output: estimate TWN (µe , Σe ) k k get f v(ˆz − x ) = TWN (ˆz − µv , Σv ) ; k k k k k k / multiply TWN (ˆz − µv , Σv ) and * k k k p p TWN (µ , Σ ) / w w w k k * scenario σ1 σ2 ρ get c, m1,1, m1,2 by numerical integration; 1n 1 1 0 atan2(Im m , Re m ) 1c 1 1 0.9 µ ← 1,1 1,1 ; 2n 1 0.1 0 atan2(Im m1,2, Re m1,2) 2c 1 0.1 0.9 p σ1 ← −2 log ||m1,1|| ; p TABLE I: System noise parameters for the four scenarios. σ2 ← −2 log ||m1,2|| ; get s12, s11, s22 by numerical integration; √ We consider four scenarios (1n, 1c, 2n, 2c) with different ρc ← s12/ s11s22; 1 −1 p 2 2  system noise parameters, equal (1) or different (2) noise in ρ ← sinh sinh(σ ) sinh(σ ) · ρc ; σ1σ2 1 2 the two dimensions, and uncorrelated (n) or correlated noise /* check result */ (c). The parameters are given in Table I. The mean of the if −1 < ρ < 1 then system noise is zero in all cases. The zero-mean measurement µe ← µ ; v v k noise has parameters σ1 = 1, σ2 = 1, ρv = 0.5.  2  e σ1 ρσ1σ2 In order to evaluate the performance, we use the angular Σk ← 2 ; ρσ1σ2 σ2 RMSE (root mean square error) over K time steps else v µe ← µp; u K k k 1   2 e p u X e e Σ ← Σ ; ej := t min (|xˆk,jµ |, 2π − |xˆk,jµ | k k K k,j k,j end k=1 return TWN (µe , Σe ); k k in both dimensions j = 1, 2 separately. The angular RMSE considers the shorter of the two possible paths between two points on a circle. 1.6 1.6 1.6 1.6 RMSE x RMSE x RMSE x RMSE x 1.4 RMSE y 1.4 RMSE y 1.4 RMSE y 1.4 RMSE y 1.2 1.2 1.2 1.2

1 1 1 1

0.8 0.8 0.8 0.8 twn twn twn twn 0.6 0.6 0.6 0.6

0.4 0.4 0.4 0.4

0.2 0.2 0.2 0.2

0 0 0 0 0 0.5 1 1.5 0 0.5 1 1.5 0 0.5 1 1.5 0 0.5 1 1.5 kf kf kf kf (a) 1n (b) 1c (c) 2n (d) 2c Fig. 3: Comparison of RMSE of Kalman filter (kf) and proposed filter (twn) over 100 Monte Carlo runs for uncorrelated system noise and correlated system noise. Values below the diagonal mean that the proposed filter is better, values above the diagonal mean that the Kalman filter is better. It can be seen that the proposed filter slightly outperforms the Kalman filter in the 1n and 2n scenarios and yields significantly better results in the 1c and 2c scenarios.

scenario # outperforming runs mean error quotient j = 1 j = 2 j = 1 j = 2 though some of the methods used in this paper can easily be 1n 73 73 1.0465 1.0520 generalized to a higher number of dimensions, this general- 1c 87 93 1.1601 1.2070 ization involves some additional challenges. Particularly, the 2n 80 55 1.0667 1.0065 2c 93 77 1.2618 1.0687 moment-based approximation of the multiplication of partially wrapped normal densities may be difficult, as the numerical TABLE II: Results from 100 Monte Carlo runs. In each integration used for calculating the toroidal moments does not dimension j = 1, 2, we give the number of runs where the scale well for a higher number of dimensions. Therefore, other proposed filter outperforms the Kalman filter, as well as the algorithms for approximating the product of multidimensional mean of the quotient of the error of the Kalman filter and wrapped normal densities may be necessary. the error of the proposed filter (values larger than 1 indicate We plan to extend the proposed filtering algorithm to that the Kalman filter performs worse). nonlinear system and measurement equations by applying deterministic sampling techniques as have been used in [7] and [23]. Furthermore, we have published some results on a We performed 100 Monte Carlos runs with K = 50 time related circular-linear distribution [24], which might be used steps each. The results are depicted in Fig. 3. It can be to generalize the toroidal filter to arbitrary partially periodic seen that the proposed filter outperforms the Kalman filter spaces. significantly, in particular in the case of correlated system noise. ACKNOWLEDGMENT As far as runtime is concerned, numerical evaluation of This work was partially supported by grants from the the integrals for c, m1,1, m1,2, s12, s11, s22 is by far the most German Research Foundation (DFG) within the Research costly step of the algorithm. Still, the algorithm is reasonably Training Groups RTG 1126 “Soft-tissue Surgery: New fast overall. On a standard computer with an Intel Core i7- Computer-based Methods for the Future Workplace” and 4770 CPU, 16 GB RAM, and MATLAB 2013b, we can RTG 1194 “Self-organizing Sensor-Actuator-Networks”. calculate all required integrals for one time step in less than 100 ms. APPENDIX

VI.CONCLUSION A. Proof of Lemma 1 In this paper, we presented a new method for estimating We introduce the abbreviation correlated angles using directional statistics. We derived a µ := µ + [2πj, 2πk]T , filter based on the toroidal wrapped normal distribution and jk evaluated its performance by comparing with a standard and calculate approach in multiple simulations. Our results suggest that Z 2πZ 2π  inx1  the proposed approach outperforms standard approaches, e f(x) inx2 dx1dx2 particularly in cases of large noise and strong correlation. To 0 0 e 2π 2π ∞ the best of our knowledge, the presented algorithms constitute Z Z X   einx1  = N x; µ , Σ dx dx the first recursive filter on the torus that is based on directional jk inx2 1 2 0 0 e statistics and correctly takes periodicity into account. j,k=−∞  2π ∞ ∞ 2π    So far, the proposed filter is limited to two angular R P P R N x; µ , Σ dx einx1 dx (a) 0 j=−∞ k=−∞ 0 jk 2 1 dimensions. Future work may include the generalization to =  2π ∞ ∞ 2π    R P P R N x; µ , Σ dx einx2 dx n correlated angles, i.e., estimation on the n-torus. Even 0 k=−∞ j=−∞ 0 jk 1 2      2π ∞ 2πj  R P∞ R inx1 [3] K. V. Mardia and P. E. Jupp, Directional Statistics, 1st ed. Wiley, j=−∞ N x; µ+ , Σ dx2e dx1 (b)  0 −∞ 0  1999. =       [4] S. R. Jammalamadaka and A. Sengupta, Topics in Circular Statistics.  2π ∞ ∞ 0  R P R inx2 World Scientific, 2001. k=−∞ N x; µ+ , Σ dx1e dx2 0 −∞ 2πk [5] M. Azmani, S. Reboul, J.-B. Choquel, and M. Benjelloun, “A Recursive 2π Fusion Filter for Angular Data,” in IEEE International Conference on "R P∞ inx1 # (c) N (x1; µ1 + 2πj, σ1) e dx1 = 0 j=−∞ Robotics and Biomimetics (ROBIO 2009), 2009, pp. 882–887. 2π ∞ R P inx2 [6] G. Stienne, S. Reboul, M. Azmani, J. Choquel, and M. Benjelloun, 0 k=−∞ N (x2; µ2 + 2πk, σ2) e dx2 Information  2  “A Multi-temporal Multi-sensor Circular Fusion Filter,” exp(inµ1 − nσ1/2) Fusion, vol. 18, pp. 86–100, Jul. 2013. = 2 . exp(inµ2 − nσ2/2) [7] G. Kurz, I. Gilitschenski, and U. D. Hanebeck, “Recursive Nonlinear Filtering for Angular Data Based on Circular Distributions,” in Proceedings of the 2013 American Control Conference (ACC 2013), Washington D. C., USA, Jun. 2013. At (a), we use the dominated convergence theorem to [8] G. Kurz, I. Gilitschenski, S. J. Julier, and U. D. Hanebeck, “Recursive interchange summation and integration. Step (b) is based Estimation of Orientation Based on the Bingham Distribution,” in on concatenation of integrals Proceedings of the 16th International Conference on Information Fusion (Fusion 2013), Istanbul, Turkey, Jul. 2013. ∞ X Z a [9] I. Gilitschenski, G. Kurz, S. J. Julier, and U. D. Hanebeck, “Unscented f(x + ka)dx Orientation Estimation Based on the Bingham Distribution,” arXiv preprint: Systems and Control (cs.SY), 2013. [Online]. Available: k=−∞ 0 Z a Z a Z a http://arxiv.org/abs/1311.5796 [10] J. Glover and L. P. Kaelbling, “Tracking 3-D Rotations with the = ··· + f(x − a)dx + f(x)dx + f(x + a)dx + ··· Bingham Filter,” MIT, Tech. Rep., Mar. 2013. 0 0 0 Z 0 Z a Z 2a [11] W. Feiten, M. Lang, and S. Hirche, “Rigid Motion Estimation using Mixtures of Projected Gaussians,” in Proceedings of the 16th = ··· + f(x)dx + f(x)dx + f(x)dx + ··· International Conference on Information Fusion (Fusion 2013), 2013. −a 0 a Z ∞ [12] J. M. Selig, Geometric Fundamentals of Robotics, 2nd ed., ser. Monographs in Computer Science, F. B. S. David Gries, Ed. New = f(x)dx . York: Springer, 2005. −∞ [13] K. V. Mardia, C. C. Taylor, and G. K. Subramaniam, “ We use marginalization of a Gaussian distribution at (c). Bioinformatics and Mixtures of Bivariate von Mises Distributions Finally, we apply the formulas for circular moments of WN for Angular Data,” Biometrics, vol. 63, no. 2, pp. 505–512, 2007. [14] H. Singh, V. Hnizdo, and E. Demchuk, “Probabilistic Model for Two distributions as given in [7], [4]. Dependent Circular Variables,” Biometrika, vol. 89, no. 3, pp. 719–723, 2002. B. Proof of Lemma 3 [15] S. R. Jammalamadaka and Y. Sarma, “A Correlation Coefficient for Proof. The equations for mean and uncertainty are obtained Angular Variables,” Statistical theory and data analysis II, pp. 349–364, 1988. by solving the system of equations [16] K. V. Mardia, “Statistics of Directional Data,” Journal of the Royal m  exp(inµ − nσ2/2) Statistical Society. Series B (Methodological), vol. 37, no. 3, pp. 349– 1,1 1 1 393, 1975. = 2 m1,2 exp(inµ2 − nσ2/2) [17] R. A. Johnson and T. Wehrly, “Measures and Models for Angular Correlation and Angular-Linear Correlation,” Journal of the Royal for µ and σ1, σ2. To get the correlation parameter ρ, we solve Statistical Society. Series B (Methodological), vol. 39, no. 2, pp. 222– 229, 1977. sinh(σ1σ2ρ) [18] P. E. Jupp and K. V. Mardia, “A General Correlation Coefficient ρc = p 2 2 for Directional Data and Related Regression Problems,” Biometrika, sinh(σ1) sinh(σ2) vol. 67, no. 1, pp. 163–173, 1980. for ρ. If ρ ∈ (−1, 1), this yields a valid solution and the [19] N. I. Fisher and A. J. Lee, “A Correlation Coefficient for Circular resulting TWN distribution has indeed the desired first toroidal Data,” Biometrika, vol. 70, no. 2, pp. 327–332, 1983. [20] M. Abramowitz and I. A. Stegun, Handbook of Mathematical Functions moment and circular correlation coefficient. If, conversely, with Formulas, Graphs, and Mathematical Tables, 10th ed. New York: ρ∈ / (−1, 1), there exists no TWN distribution with the desired Dover, 1972. properties, because it is a necessary condition that ρ fulfills [21] A. Genz, “Numerical Computation of Rectangular Bivariate and Trivariate Normal and t Probabilities,” Statistics and Computing, vol. 14, this equation. The intuition why such a TWN distribution no. 3, pp. 251–260, 2004. does not necessarily exist is depicted in Fig. 2b. [22] L. F. Shampine, “Matlab Program for Quadrature in 2D,” Applied Mathematics and Computation, vol. 202, no. 1, pp. 266–274, 2008. REFERENCES [23] U. D. Hanebeck, “PGF 42: Progressive Gaussian Filtering with a Twist,” in Proceedings of the 16th International Conference on Information [1] R. E. Kalman, “A New Approach to Linear Filtering and Prediction Fusion (Fusion 2013), Istanbul, Turkey, Jul. 2013. Problems,” Transactions of the ASME Journal of Basic Engineering, [24] G. Kurz, I. Gilitschenski, and U. D. Hanebeck, “The Partially Wrapped vol. 82, pp. 35–45, 1960. Normal Distribution for SE(2) Estimation (to appear),” in Proceedings [2] S. J. Julier and J. K. Uhlmann, “Unscented Filtering and Nonlinear of the 2014 IEEE International Conference on Multisensor Fusion and Estimation,” Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, Information Integration(MFI 2014), Beijing, China, Sep. 2014. Mar. 2004.