Bivariate Angular Estimation Under Consideration of Dependencies Using Directional Statistics 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 probability distribution 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 Bingham distribution [8], [9], which captures correlations correlations may exist between those angles and have to be when estimating rotations represented as quaternions. 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 von Mises distribution 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 normal distribution 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 =fx 2 C : jjxjj = 1g =fcos(φ) + i sin(φ) : 0 ≤ φ < 2πg 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 2 C ; A univariate wrapped normal (WN) distribution is given by 0 its probability density function (pdf) where i is the imaginary unit. 1 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 2 S , location parameter µ 2 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 2 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 1 1 " # " 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] 2 S × S , location parameter µ = i.e., the componentwise circular moment of a WN (µ1; σ1) T 1 1 [µ1; µ2] 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 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.