METHODS for DETERMINISTIC APPROXIMATION of CIRCULAR DENSITIES 139 DEFINITION 4 (Wrapped Dirac Distribution)
Total Page:16
File Type:pdf, Size:1020Kb
1. INTRODUCTION Many estimation problems involve circular quanti- ties, for example the orientation of a vehicle or the angle Methods for Deterministic of a robotic joint. Since conventional estimation algo- rithms perform poorly in these applications, particularly Approximation of Circular if the angular uncertainty is high, circular estimation methods such as [31], [34], [35], [6], [54], [57], and Densities [43] have been proposed. These methods use circular probability distributions stemming from the field of di- rectional statistics [21], [42]. Circular estimation methods have been applied to a variety of problems in different fields. For example, GERHARD KURZ many signal processing applications necessitate the con- IGOR GILITSCHENSKI ROLAND Y. SIEGWART sideration of circular quantities. Consider for instance UWE D. HANEBECK phase estimation and tracking [39], [7], signal process- ing for global navigation satellite systems (GNSS) [54], [53], [26], and azimuthal speaker tracking [57]. In me- teorology, estimation of the wind direction [11], [8] is Circular estimation problems arise in many applications and can of interest and in aerospace applications, the heading be addressed with methods based on circular distributions, e.g., the of an airplane may be estimated [35]. Through a suit- wrapped normal distribution and the von Mises distribution. To able mapping, constrained object tracking problems on periodic one-dimensional manifolds can also be inter- develop nonlinear circular filters, a deterministic sample-based ap- preted as circular estimation problems [29]. Finally, cir- proximation of these distributions with a so-called wrapped Dirac cular densities arise naturally in bearings-only tracking mixture distribution is beneficial. We present a closed-form solu- [12], [44]. tion to obtain a symmetric wrapped Dirac mixture with five com- To facilitate the development of nonlinear filters, ponents based on matching the first two trigonometric moments. sample-based approaches are commonly used. The rea- Furthermore, we discuss the choice of a scaling parameter involved son is that samples, which we represent as Dirac delta in this method and extend it by superimposing samples obtained distributions, can easily be propagated through nonlin- from different scaling parameters. Finally, we propose an approxi- ear functions. We distinguish deterministic and nonde- mation based on a binary tree that approximates the shape of the terministic approaches. In the noncircular case, typical true density rather than its trigonometric moments. All proposed examples for deterministic approaches include the un- scented Kalman filter (UKF) [24] as well as extensions approaches are thoroughly evaluated and compared in different sce- thereof [56], the cubature Kalman filter [4], [23], [22], narios. andthesmartsamplingKalmanfilter(S2KF) [52]. Non- deterministic filters for the noncircular case are the par- ticle filter [5], the Gaussian particle filter [25], and the randomized UKF [55]. We focus on deterministic approaches because they have several distinct advantages. First of all, as a result of their deterministic nature, all results are reproducible, Manuscript received December 4, 2015; revised June 17, 2016; re- i.e., for the same input (e.g., measurements and initial leased for publication July 26, 2016. estimate), deterministic filters will always produce the 1 Refereeing of this contribution was handled by Chee-Yee Chong. same output. Second, the samples are placed accord- ing to certain optimality criteria (i.e., moment matching Authors’ addresses: G. Kurz and U. Hanebeck, Intelligent Sensor- Actuator-Systems Laboratory (ISAS), Institute for Anthropomatics [24], shape approximation [16], [50]), or a combina- and Robotics, Karlsruhe Institute of Technology (KIT), Germany (E- tion thereof [19], [17]. Consequently, a much smaller mail: [email protected], [email protected]). I. Gilitschen- number of samples is sufficient to achieve a good ap- ski and R. Siegwart, Autonomous Systems Laboratory (ASL), In- proximation. Third, nondeterministic approaches usu- stitute of Robotics and Intelligent Systems, Swiss Federal Institute ally have a certain probability of causing the filtering of Technology Zurich, Switzerland (E-mail: [email protected], algorithm to fail due to a poor choice of samples. This [email protected]). is avoided in deterministic methods. This is an extended version of the paper “Deterministic Approxima- tion of Circular Densities with Symmetric Dirac Mixtures Based on 1Randomized approaches can be made reproducible by choosing a Two Circular Moments” [32] published at the 17th International Con- fixed seed for the random number generator. However, this choice is ference on Information Fusion (Fusion 2014), which received the Jean- completely arbitrary and affects the performance. Also, minor changes Pierre Le Cadre Award for Best Paper. to the implementation, e.g., the order in which certain random num- bers are drawn or the choice of the underlying random number gen- 1557-6418/16/$17.00 c 2016 JAIF erator, will affect the result. ° 138 JOURNAL OF ADVANCES IN INFORMATION FUSION VOL. 11, NO. 2 DECEMBER 2016 In our previous publication [31], we presented a de- terministic approximation for von Mises and wrapped normal distributions with three samples. This approx- imation is based on matching the first trigonometric moment. The first trigonometric moment is a complex number and a measure of both location and dispersion. This approximation has already been applied to con- strained object tracking [29], sensor scheduling based on bearing-only measurements [12], as well as stochas- tic model predictive control [28]. The contributions of this paper can be summarized as follows. We present an extension of our previous Fig. 1. Probability density functions of WN, WC, and VM approach [31] to match both the first and the second distributions with identical first trigonometric moment. trigonometric moment, which was first discussed in [32]. This yields an approximation with five samples. distribution on the circle, i.e., in a circular setting, it Even though this approximation is slightly more com- is reasonable to assume that noise is WN distributed. plicated, it can still be computed in closed form and does To see this, we consider i.i.d. random variables μ with not require any numerical computations or approxima- i E(μ ) = 0 and finite variance. Then the sum tions. We have previously applied this method to the i problem of heart phase estimation in [39]. 1 n S = μ The algorithm from [32] requires choosing a param- n pn X k eter ¸ [0,1]. In this paper, we will show that the choice k=1 2 ¸ =0:8 ensures good approximations even when the ap- converges to a normally distributed random variable if proximated distribution converges to a uniform distribu- n . Consequently, the wrapped sum (Sn mod 2¼) tion. converges!1 to a WN-distributed random variable. Numer- Furthermore, we present a novel superposition ical computation of the pdf is discussed in [33]. method that is able to combine sample sets with dif- ferent choices of ¸ in order to obtain a larger number DEFINITION 2 (Wrapped Cauchy Distribution). The of samples while still maintaining the first and second wrapped Cauchy (WC) distribution [21], [42] has the trigonometric moment. pdf 1 1 ° Finally, we also propose a new method based on the f(x;¹,°)= , shape of the probability distribution function rather than ¼ X °2 +(x ¹ +2k¼)2 k= ¡ its moments. This method creates a binary tree consist- ¡1 where ¹ [0,2¼)and°>0. ing of intervals in [0,2¼) and distributes the samples in 2 proportion to the probability mass contained in the inter- Similar to the WN distribution, a WC distribution val. Unlike previous shape-based methods such as [17], is obtained by wrapping a Cauchy distribution around the proposed method does not require numerical opti- the circle. Unlike the WN distribution, it is possible mization. Thus, it is very fast, provided an efficient al- to simplify the infinite sum in this case, yielding the gorithm for calculating the cumulative distribution func- closed-form expression tion of the respective density is available. 1 sinh(°) f(x;¹,°)= : 2. PREREQUISITES 2¼ cosh(°) cos(x ¹) ¡ ¡ In this section, we define the required probability DEFINITION 3 (Von Mises Distribution). A von Mises distributions (see Fig. 1) and introduce the concept of (VM) distribution [58] is defined by the pdf trigonometric moments. 1 f(x;¹,·)= exp(·cos(x ¹)), DEFINITION 1 (Wrapped Normal Distribution). A wrap- 2¼I0(·) ¡ ped normal (WN) distribution [48] is given by the where ¹ [0,2¼)and·>0 are parameters for location probability density function (pdf) and concentration,2 respectively, and I ( ) is the modified 0 ¢ 1 1 (x ¹ +2k¼)2 Bessel function of order 0. f(x;¹,¾)= exp μ ¡ ¶, p X ¡ 2¾2 2¼¾ k= According to [1, eq. 9.6.19], the modified Bessel ¡1 function of integer order n is given by where ¹ [0,2¼)and¾>0 are parameters for center and dispersion,2 respectively. 1 ¼ I (z)= Z exp(z cosμ)cos(nμ)dμ: n ¼ The WN distribution is obtained by wrapping a one- 0 dimensional Gaussian density around the unit circle. It The von Mises distribution has a similar shape as a WN is of particular interest because it appears as a limit distribution and is frequently used in circular statistics. METHODS FOR DETERMINISTIC APPROXIMATION OF CIRCULAR DENSITIES 139 DEFINITION 4 (Wrapped Dirac Distribution). A wrap- ped Dirac mixture (WD) distribution has the pdf L f(x;w ,:::,w ,¯ ,:::,¯ )= w ±(x ¯ ), 1 L 1 L X j ¡ j j=1 where L is the number of components, ¯ ,:::,¯ 1 L 2 [0,2¼) are the Dirac positions, w1,:::,wL > 0arethe weighting coefficients and ± is the Dirac delta distribu- tion [31]. Additionally, we require L w =1 toen- Pj=1 j sure that the WD distribution is normalized. Fig. 2. First trigonometric moment of wrapped normal, wrapped Cauchy, and von Mises distributions with zero mean plotted against Unlike the continuous WN, WC and VM distribu- their second trigonometric moment. The moments are real-valued in tions, the WD distribution is a discrete distribution con- this case because ¹ =0.