Directional Statistics for 3D Model-Based UAV Tracking

We present a vision system based on a monocular camera to track the 3D position and orientation of an Unmanned Aerial Vehicle (UAV) during the landing process aboard a ship. The proposed method uses a 3D model-based approach based on a Particle Filter (PF) with proposal distributions given by an Unscented Kalman Filter (UKF) for the translational motion and filters based on directional statistics for the rotational motion. Our main contributions are (i) the development of a novel 3D model-based tracking architecture based on directional statistics that can be easily adapted to other tracking problems, and (ii) the development of the Unscented Bingham-Gauss Filter (UBiGaF) for rotation estimation. We show the advantages of using directional statistics based filters on 3D model-based tracking in a series of quantitative tests in a challenging simulation scenario with real video data. The obtained position and angular error are compatible with the automatic landing system requirements when using directional statistics. We obtain lower error when using the UBiGaF scheme for the vast majority of the tested combinations.


I. INTRODUCTION
The vast majority of the accidents and incidents with Unmanned Aerial Vehicles (UAVs) occur during take-off or landing [1], [2], since these are the most challenging maneuvres where, typically, an external pilot takes control. Reducing human intervention in these operations increases system reliability and alleviates the use of trained UAV pilots.
We propose one approach to automate the landing maneuver by tracking the pose of the UAV with a monocular Red, Green and Blue (RGB) camera located on the Fast Patrol Boat (FPB) with a processing station to perform the needed Computer Vision (CV) processing tasks. A groundbased vision system makes it possible to use more processing power, allowing the use of more computationally intensive methods. On the contrary to systems that embed the sensing and processing on the UAV, our approach does not require any customization of standard Commercial-Off-The-Shelf (COTS) UAVs. The UAV platform is composed of sensors, actuators, communications, and onboard processing, and the Ground Control Station (GCS) is composed of communi-The associate editor coordinating the review of this manuscript and approving it for publication was Yuming Fang . cations, a camera, and a processing station with a tracking algorithm (Figure 1). The system observes and obtains the UAV pose and sends that information to the GCS that computes the needed control commands to guide the UAV via radio to perform autonomous landing using a net-based retention system ( Figure 2). In out scenarios, the fact that the camera is installed in a moving platform in a maritime environment and the reduced dimensions of the fixed-wing UAV makes the initial detection and the tracking very challenging tasks.
In previous work [3] we have proposed a pose tracking framework using a 3D model-based vision system employing the UAV Computer-Aided Design (CAD) model and an Unscented Particle Filter (UPF) based approach using Gaussian noise in the translational component of motion and Bingham (Bi) noise in the rotational component, to better model the periodic nature of rotations. However, the Bi noise is specific to the angular position component and does not capture its correlation with the angular velocity. In this paper, we introduce Bingham-Gauss (BiGa) noise to model the full rotational noise, both in its angular position and velocity components. We add new results that show significant performance improvements when using the BiGa model with respect to the Bi model (up to 50% less pose estimation error). In the current implementation, we divide the translation and rotation into two independent motions. In the UAV field, this decoupling has been applied with success to automatic landing using the airborne camera [4], using stereo vision to estimate altitude, attitude and motion [5] and UAV controller design [6]. For filtering the rotational motion, the Gaussian model is not a good approximation in the presence of strong orientation noise. The Bi distribution (Appendix B) is one distribution employed in the field of directional statistics a subdiscipline of statistics that deals with directions. Some examples of data that may be considered as directional statistics are orientations, compass directions and time of day. The BiGa distribution (Appendix D), consists in the product of a Bi distribution and a Gaussian distribution conditioned on the Bi distributed random variables [7]. This distribution allows us to capture dependencies between the angular velocity and the UAV attitude. The motion filtering is made using an Unscented Kalman Filter (UKF) for the translation [8] (Appendix A) and an Unscented Bingham Filter (UBiF) [3], [9]- [12] (Section IV-D) or a new filter using the BiGa distribution, denoted Unscented Bingham-Gauss Filter (UBiGaF) (Section IV-E) for the rotation.
The main contributions of this article are (i) the inclusion of directional statistics and correlation between attitude and angular velocity in the UAV tracking field, (ii) the development and analysis of a novel UBiGaF for the state estimation of rotating objects, (iii) the use of directional statistics in 3D model-based tracking and (iv) the comparison of performance between the UPF, the UBiF and the UBiGaF in a simulation environment using real data.
This article is organized as follows. In Section II, the related work regarding 3D Model-based pose estimation and pose tracking is outlined. In Section III, the overall system proposal is outlined, describing the used target detection and tracking methods. In Section IV, the used motion models and Unscented filters are explained in detail. In Section V, we present the experimental results. Finally, in Section VI, we present the conclusions and provide directions for further research work.

II. RELATED WORK
This section makes a review of the related work with particular focus on the 3D Model-based pose estimation (Section II-A) and pose tracking (Section II-B).

A. 3D MODEL-BASED POSE ESTIMATION
Pose estimation is an important task and has been studied in several fields of science. The class of methods that use the object or environment CAD model to perform pose estimation are called 3D model-based pose estimation. The main applications of 3D model-based pose estimation are Augmented Reality (AuRe) [13], robot manipulation [14], robot navigation [15], object pose tracking [16], among others.
Using the RGB onboard camera and the environment 3D CAD model we can perform feature matching [15] using the moving edges tracker algorithm for pose estimation and combine this information with Inertial Measurement Unit (IMU) data to get the translational velocities needed for the UAV control [17]. In [18], [19], we describe a ground-based vision system that estimates the UAV pose using its CAD model. The vast majority of the ground-based systems are developed for tracking or control without using CAD information. Nowadays all UAVs have their 3D CAD model available, so their pose can be estimated using a class of methods for 3D model-based pose estimation. We propose a 3D modelbased ground-based vision system with high processing capability, which allows reducing the UAV size, weight and power requirements. This approach makes it also possible to use standard UAVs equipped with COTS autopilots.

B. POSE TRACKING
Given several measures over time, we can use target specific dynamic models to filter the sensor data using a Kalman Filter (KF) [20], an Extended Kalman Filter (EKF) [21], a UKF [22] or a Particle Filter (PF) [23]. PFs in CV were applied to visual tracking tasks such as ball tracking [24], spherical pendulum tracking [25], human face tracking [26], articulated object tracking [27], vehicle tracking [28] or object tracking [29]. The combination of a PF with a UKF, known as a UPF [30], is described in [31] for visual contour tracking, and in [32] for ground maneuvering target tracking. The UKF generates better proposal distributions for the PF, taking into account the current observations [33].
When using conventional filtering techniques for attitude estimation, we have to consider a small angle assumption to quantify the existing uncertainty [34]. To overcome this, we can use a directional distribution to take into account the periodic nature of the rotation. The Bi distribution [35] is defined directly on the unit multidimensional hypersphere [36]. It has been successfully used to (i) estimate the attitude of a ping pong ball [37], (ii) for estimation of orientations in the 3D space with unit quaternions [38] and (iii) using a UKF based approach using deterministic sampling [9]. Some mixtures and combinations of distributions have also been made to quantify the correlation between Euclidean states (e.g. angular velocities) and the attitude on its manifold. Some of these implementations are the Partially-Conditioned Gaussian Mixtures (PCGM) [34], the Gauss-Bingham (GaBi) [10] and the Bingham-Gauss (BiGa) [7] distributions. These approaches allow us to capture the covariance between the attitude and angular velocity uncertainties.
As described in Section I, we have developed a pose tracking architecture based on a UPF. The main developments regarding our previous work [3], [12] is a comparison among several tracking architectures based on UPFs [8] and the development of a novel UBiGaF able to capture the covariance between attitude and angular velocity.

III. SYSTEM PROPOSAL
This section presents the overall system architecture used to perform UAV tracking. This architecture has been developed and refined in the last few years in a series of publications [3], [8], [12], [18], [19]. In this paper, we propose modifications to the tracking modules using directional statistics.
As described in Section I, we are using a UPF based scheme where each filter particle will represent the UAV state -linear and angular positions and velocities: where is the orientation quaternion, and ω T t = [ω x , ω y , ω z ] is the angular velocity according to the camera reference frame described in Figure 3. The state distribution is represented by the particle set x i t . The developed system can be divided into two parts ( Figure 4): • Detection and Hypotheses generation (Section III-A); • Tracking (Section III-B).

A. DETECTION AND HYPOTHESES GENERATION
The initial UAV detection is made using a You Only Look Once (YOLO) classifier [39] trained on a created synthetic dataset performing transfer learning to real images. After we obtain the Region Of Interest (ROI), we compare it with a pretrained database of synthetic images of the UAV in multiple poses to retrieve a set of top-ranked poses as described in [18].

B. TRACKING
The standard UPF [30] scheme is divided into (i) initialization, (ii) importance sampling, and (iii) importance weighting and resampling. The initialization is possible using the detection and hypotheses generation stage (Section III-A). The main difference, when compared with the generic UPF, is in the importance sampling step, where we use a UKF-UBiF/UBiGaF (Section IV) to integrate the current observation and generate a better proposal distribution. The current observation z t = [t T t , q T t ] T is given by the maximum likelihood particle according to a color similarity metric [18]. The applied resampling strategy was resampling reallocation [19]. The mode of the distribution, approximated by the particle with the largest importance weight before resampling, is used as the state estimation. If during tracking, we cannot detect the UAV on the frame, all the particles came from the last iteration. The main article focus will be the motion filtering rotational part (Section IV).

IV. MOTION FILTERING
Motion filtering has the objective of using measures affected by uncertainty and noise over time and generate results closer to the real values of the measured quantities. We have considered a constant velocity model with independent linear and angular motions [40]. The translational motion (Section IV-A) filtering will be performed using a UKF (Appendix A) and the rotational motion (Section IV-B) filtering will be performed using a UBiF (Section IV-D) or a UBiGaF (Section IV-E).

A. TRANSLATIONAL TRANSITION MODEL
As we are using a linear model for translation, a simple KF could be applied. To facilitate the transition between the linear and the angular filter formulations, we will use a discrete-time UKF. This is a very common filter, so we remit its description to Appendix A. In discrete time, the evolution of the state is: is a Gaussian noise random variable with zero mean and covariance Q l t .

B. ROTATIONAL TRANSITION MODEL
We represent the UAV rotation using a unit quaternion: where q ∈ S 3 ⊂ R 4 : || q || = 1 with: whereê is the axis rotation and ρ is the angle of rotation. In discrete time, the evolution of the state is [8]: where x r t+1 = r t+1 (1), ⊗ represents unit quaternion multiplication (orientations composition), ξ r t ∼ N (0, Q r t ) is a Gaussian noise random variable with zero mean and covariance Q r t , and δq ω t and δq r t are quaternions representing the integration of the effect of the angular velocity and rotation noise assumed constant during a sampling interval t: with: The observation model is given by: is a Gaussian noise random variable with zero mean and covariance matrix R t and δq η t is a quaternion representing the integration of the effect of the observation rotation noise in a similar way to (6). When using the UBiF (Section IV-D) or the UBiGaF (Section IV-E), the noise of the rotational components of the observation model is assumed Bi distributed (Appendix B).

D. UNSCENTED BINGHAM FILTER (UBIF)
In a periodic domain like the manifold of orientations in a 3D space, the Gaussian model is not a good approximation, especially in the presence of strong noise. The Bi distribution is an antipodally symmetric distribution that represents a zero-mean Gaussian distribution projected on the unit hypersphere (Appendix B). Since the product of two Bi distributions is closed under multiplication after renormalization (Appendix B-B), we can use the UBiF with an update step directly derived from the Bayes filter formulation. We apply a UBiF to the orientation part of the state vector. As in other filtering frameworks, the used UBiF has two stages: (i) prediction (Section IV-D.1), and (ii) update (Section IV-D.2). The angular velocities will be obtained from the orientation difference between iterations. The UBiF schematic view is described in Figure 5.

1) PREDICTION
The prediction step is described in Algorithm 1. For the rotation case, the system model is given by: is the Bi distributed system noise where M is an orthogonal matrix describing the orientation of the distribution and Z is the concentration matrix that controls the spread of the distribution around its mean (Appendix B). The system dynamic F(.) is given by (5).

2) MEASUREMENT UPDATE
The measurement update step is described in Algorithm 2. The measurement model is represented as: where z t ∈ S 3 is the measurement at time t and t ∼ P B M t , Z t is the Bi distributed measurement noise. The current observation z t is given by the orientation quaternion of the maximum likelihood particle, as described in To be able to apply the measurement update step (Algorithm 2), the noise is rotated (disturbed) according to the actual measurement z t according to (43): where P B (M, Z t ) represents the measurement noise P B M t , Z t with a peak aligned with z t and spread Z t . The estimate is described by P B (M e t , Z e t ), directly obtained by the product of the Bi distributed system state P B (M t ,Z t ) with the Bi disturbed measure P B (M, Z t ) (Algorithm 2). The update quaternion q t is obtained from the P B (M e t , Z e t ) mode (Algorithm 2). The quaternion error is obtained by multiplying the previous quaternion (q t−1 ) with the conjugate of the estimated one (q t ). The angular velocities are obtained converting to the angle-axis representation, according to: where t is the sampling interval and q e = q t−1 ⊗q t = [q 1 , q 2 , q 3 , q 4 ] T (4).

Algorithm 2 UBiF -Update
Step Initialization: t (10) Inputs:M t ,Z t (Algorithm 1) and z t (10) 1. Disturb the measure z t using the Bi noise t (11): The measurement update can be derived from the Bayes theorem: where C is a normalization constant; 3. The final estimate is then obtained from (42): The UBiF does not quantify the uncertainty of the correlation between angular velocity ω and the quaternion attitude q on their natural manifold. We will use the BiGa distribution (Appendix D) that allows to capture this correlation in a filtering structure. The BiGa is a distribution that consists in the product of a Bi distribution and a Gaussian distribution conditioned on the Bi distributed random variables. Thus, we propose the Unscented Bingham-Gauss Filter (UBiGaF). As described in the Section IV-D, the multiplication of two Bi distributions is closed under multiplication after renormalization, but the same did not happen using the BiGa distribution since we do not have a closed-form multiplication. To be able to incorporate it on a filtering structure, we developed an update step that was based on the Unscented Transform (UT) [30], [31] with a structure similar to the UKF. The UBiGaF is separated into: (i) prediction (Section IV-E.1), and (ii) update (Section IV-E.2). The UBiGaF schematic view is described in Figure 6.

1) PREDICTION
The prediction step is described in Algorithm 3. The system state at time t − 1 is given by: where q t−1 is the attitude quaternion and ω t−1 is its angular velocity at time instant t − 1. The state vector is assumed to be BiGa distributed with parameters defined by m ω t−1 (60), The system model is given by: where t−1 ∼ P BG with Covariance P τ t−1 for the angular velocity part and Covariance t−1 ∼ P t−1 ∼ P B (M t−1 , Z t−1 ) for the orientation part, F(.) is the motion model (5) and represents the BiGa composition obtained using the sigma points representation (Appendix E).
The quaternion motion q M = [q 1 , q 2 , q 3 , q 4 ] T (4) used to propagate each one of the sigma pointsẐ i t−1 (Algorithm 3) is given by (7): where f ω (Ẑ i t−1 ) = [ω x , ω y , ω z ] T and t is the sampling interval. We calculate the BiGa parameters from the obtained sigma points after propagationẐ i t (Appendix E). After the prediction step (Algorithm 3), the predicted system state is described by a BiGa distribution with parameters defined bym ω t ,P ω t ,P q t ,P qω t .

2) MEASUREMENT UPDATE
The measurement update step is described in Algorithm 4, and is similar to the used in the UBiF (Section IV-D.2). The measurement model is also represented as: where z t ∈ S 3 is the current observation at time t and t ∼ P B M t , Z t is the Bi distributed measurement noise. Function H(.) relates the measurement z t to the values of the orientation q t (identity function in our study case).
Using the BiGa formulation, we can model the full rotational noise, both in its angular position and velocity components. As initially described in Section IV-E, we do not

Algorithm 3 UBiGaF -Prediction
Step Obtain sigma points Z i t−1 (Appendix E); 3. Add uncertainty P τ t−1 to the angular velocity covariance: have a closed-form multiplication for the product of two BiGa distributions. To be able to deal with that, we have adopted a structure similar to the UKF (Appendix A). Using this filtering structure, we can obtain the a posteriori state estimate r t and state covariance P r t , as described in Algorithm 4. Contrarily to the UBiF, we do not need to estimate the angular velocities from the quaternion difference since we estimate them directly in the filter.

V. EXPERIMENTAL RESULTS
In this section, we show results of UAV tracking in a simulated environment using real video footage. All the developments were made on a 3.70 GHz Intel i7-8700K Central Processing Unit (CPU) using an NVIDIA Quadro P5000 Graphics Processing Unit (GPU). We have used C++ with VOLUME 8, 2020 Open source Computer Vision library (OpenCV) and Compute Unified Device Architecture (CUDA). To obtain ground truth information with real images, our simulator renders the UAV CAD model in a real captured video sequence as illustrated in Figure 7. The trajectory of the UAV during the landing sequence can be seen in Figure 8.

A. TESTS DESCRIPTION
We have tested three different filter combinations (see Table 1). In combination 1, we apply a UKF for both translation and rotation filtering [8]. In combination 2, we apply a UKF for the translational motion filtering and a UBiF for the rotational motion filtering [3], [12]. In combination 3, we apply a UKF for the translational motion filtering and the novel UBiGaF for the rotational motion filtering. We also test different particle budgets N = {50, 100, 300, 500, 1500}.
In the first filter iteration, all the particles come from the hypotheses generation scheme (Section III-

B. PERFORMANCE METRICS
On each iteration, the state estimation ( Figure 4) is given by an approximation to the mode of the distribution given by the particle with the highest importance weight before resampling. The translation error was obtained using the Euclidean distance, and the rotation error was obtained according to: where R g corresponds to our ground truth rotation matrix and R r corresponds to the retrieved rotation matrices. We have also obtained the Standard Deviation (SD), the Mean Absolute Error (MAE) and the Root Mean Square Error (RMSE).

C. TRANSLATION ERROR ANALYSIS
When we analyze Table 2, we can see that the lowest 90% error interval, RMSE and SD happens when we use, as expected, the highest number of particles. From the tested combinations, UKF+UBiGaF presents the best overall translation performance since the error starts getting close to zero when we get closer to the landing area as plotted in Figure 9. The particle perturbation caused by the correlation between angular velocity and attitude adds more diversity to the set, increasing the precision of the obtained estimate.

D. ROTATION ERROR ANALYSIS
As described in Table 3, the rotation error is higher in the UKF combination where we have the largest 90% error intervals and the highest MAE and SD values. This error is mainly motivated by the poor orientation Gaussian model approximation. When comparing UKF+UBiF and UKF+UBiGaF,     the rotation error is higher in the vast majority of combinations for UKF+UBiF. When we increase the number of particles to N = 1500, the rotation error in UBiF is higher by approximately 50% when compared with the UBiGaF. In one overall analysis, the obtained rotation error in both combinations starts getting close to zero when we get closer to the landing area ( Figure 10). The rotation error for UKF, UKF+UBiF and UKF+UBiGaF using N = 1500 can be seen in Figure 9 and Figure 10. An example of real video tracking for UBiGaF using N = 1500 can be seen in Figure 11.

VI. CONCLUSION AND FUTURE WORK
A ground-based 3D model-based approach for UAV tracking using Unscented filters was introduced and tested. The proposed tracking architecture incorporate directional statistics on a UPF scheme, combining a UKF for translational motion filtering with a UBiF or UBiGaF for rotational motion filtering. The developed new filter UBiGaF was able to represent the covariance between the angular position and velocity, obtaining better results than previous approaches. The UBiF rotation error is 50% higher when compared with the UBiGaF when we use 1500 particles. Future work will focus on the inclusion of aerodynamics constraints in the UAV motion model.

APPENDIXES APPENDIX A UNSCENTED KALMAN FILTER
In the UKF, a Gaussian approximation to the distributions of the n-dimensional state and process noises are used to generate a set of points (sigma points) that are sufficient to represent their statistics using a UT. The process noise covariance Q t−1 (n × n matrix) and the state covariance P t−1 (n × n matrix) are transformed into a 2n set of points δx t−1 (i) that represent perturbations to the current state according to: (23) the parameter ι is a scaling parameter given by ι = α 2 (n + k), where α is a positive real (0 ≤ α ≤ 1) parameter that controls the high order effects resulting from the existing nonlinearity, k is another real parameter (k ≥ 0) that will control the distance between the sigma points and their average [41]. The matrices P t−1 and Q t−1 are symmetric and positive definite, so it is possible to use the Cholesky decomposition to compute ι · (P t−1 + Q t−1 ) [42]. The computation of the sigma points X i is now done by adding directly δx t (i) to the mean value of the state vector x t according to: The process model F(.) is then applied to the obtained sigma points, generating the transformed sigma points: No additional noise is considered at this step because the noise was already added at the sigma point's creation step (24). The a priori state estimate is obtained calculating the mean of the transformed sigma points X i according to: The weights are given by [43]: with λ given by λ = α 2 (n + k) − n.
To estimate the a priori state covariance each propagated sigma point is removed from its mean to create the set of error vectors: then: where the scaling weights W c i are given by (27), except W c 0 alternatively given by [44]: where β is a non-negative term which incorporates knowledge of the higher order moments(the chosen α and β determine the accuracy of third and higher order moments for non-Gaussian inputs [43]) of the distribution [41]. The transformed sigma points are now projected into the measurement space according to: The measurement expected value is computed as:

B. MEASUREMENT UPDATE
The measurement covariance estimate P zz t is given by: The innovation vector ν t is obtained comparing the actual measurement z t to the measurement estimatez t : The innovation covariance P νν t is obtained adding the measurement noise R t to the measurement covariance P zz t : The cross-correlation matrix P xz t is obtained from Z i and X i , according to: The Kalman gain is then computed from: Finally, the a posteriori state estimate is obtained according to: (38) and the state covariance P t is given by:

APPENDIX B BINGHAM DISTRIBUTION
The Bi distribution is an antipodally symmetric distribution (opposite points on S have equal probability) that represents a zero-mean Gaussian distribution in R d projected on the unit hypersphere S d−1 [9], [35]. The Probability Density Function (PDF) for the Bi distribution is obtained by [35]: where q ∈ S d−1 ⊂ R d : || q || = 1 is a unit vector (when using quaternions d = 4), M ∈ R d×d is an orthogonal matrix (a square matrix whose columns and rows are orthonormal vectors) describing the orientation of the distribution, F(Z) is the normalization constant and Z = diag(z 1 , z 2 , . . . , z d−1 , 0) with nondecreasing negative diagonal elements is the concentration matrix that controls the spread of the distribution around its mean. Adding a multiple of the identity matrix to Z or changing the order of a column of M and the corresponding Z columns do not change the distribution, so we can force the last entry of Z to be zero for computational simplicity, and because of this, the last column of M represents the distribution mode [35], [38].

A. NORMALIZATION CONSTANT
The main difficulty in the utilization of the Bi distribution consists in the computation of the normalization constant since the distribution must integrate to one over its domain. The normalization constant is obtained by: (41) where F(Z) does not depend on the matrix M, since the orientation of the distribution peaks does not change its value. Since we are using this distribution in a real-time approach, we choose to interpolate tabulated values from a precomputed lookup table for computational efficiency. VOLUME 8, 2020

B. PRODUCT
The product of two Bi distributions is closed under multiplication after renormalization and is given by [37]: where F(Z) is the new normalization constant, M are the unit eigenvectors and D are the eigenvalues on the diagonal in ascending order of (M 1 Z 1 M T 1 +M 2 Z 2 M T 2 ), Z = D−λ dd I d×d and λ dd is the largest eigenvalue.

C. ROTATION
When we are in S 3 is possible to change (rotate) the orientation of a Bi distribution P B (q; M, Z) (40) by a fixed quaternion g ∈ S 3 according to [9], [11], [38]: where ⊗ represents the composition of orientations, M ⊗ g ≡ [m 1 ⊗ g, m 2 ⊗ g, m 3 ⊗ g , m 4 ⊗ g] and m are the columns of M. Since the quaternion multiplication is not commutative we have that r ∼ P B (g ⊗ M, Z) when r = g ⊗ q.

D. COVARIANCE
The covariance is a sufficient statistics for the Bi distribution since the Bi distribution is the maximum entropy distribution on the hypersphere which matches the sample inertia matrix [45]. The covariance of the Bi PDF is given by [35]: where (E(q)) 2 = 0 is a consequence of the antipodal symmetry and E(qq T ) is given by: where the values of the gradient of F with respect to Z are precomputed and accessed by interpolation as made for the normalization constant. The covariance of the composition (composition is a directional analog to the addition of random vectors in linear space) of two Bi distributions can be obtained using the method of moments [11]. Using this method, we can approximate the resulting composition covariance directly from their covariance matrices combination.

E. INFERENCE
It is possible to estimate the parameters of a Bi distribution which approximates a set of samples [35]. The inertia matrix for a set of N samples q = [q 1 , . . . , q N ] is given by [35]: The Maximum Likelihood Estimation (MLE)M for a set of samples is an eigenvalue problem since the columns ofM are eigenvectors κ of S [35]. The MLEẐ can be found setting the partial log-likelihood function on Z to zero. This leads to: where κ j are the eigenvectors of S. This calculation can be made using the Constrained Optimization BY Linear Approximations (COBYLA) algorithm [46].

F. SAMPLING
Is hard to sample directly from the Bi distribution because of the normalization constant. To solve this problem is used a Metropolis-Hasting sampler [47] with proposal distribution given by a projected zero-mean Gaussian with covariance S (either from (45) or (46)) and target distribution provided by the Bi density [11], [35].

APPENDIX C BINGHAM FILTER SIGMA POINTS CREATION
The created sigma points follow the same principle as used in the UT applied in the UKF for the translation case (Appendix A). We need to use 4d −2 samples that correspond in this case to fourteen samples (d = 4). Since the distribution is antipodally symmetric, it is sufficient to consider only one pole (adapting the respective weights). The canonical sigma points are given by: 3,4 = [0, ± sin α 2 , 0, cos α 2 ] T (49) q 5,6 = [0, 0, ± sin α 3 , cos α 3 ] T (50) whereq 7 is the sample located on the pole (identity quaternion). The canonical distribution will be employed since it simplifies the needed mathematical approach because the parameters will be dimensionless. The covariance of the estimated Bi distribution is obtained by (45): The deviation for each one of the canonical sigma points is obtained from α i : The weights of the sigma points are given by: where N is equal to the number of used sigma points and w 7 is the weight for the central sigma point. Each canonical sigma pointq is multiplied by M (in the UBiF we use M e tq as described in Section IV-D) originating the set of sigma points q that represent our Bi distribution P B . The sigma point propagation is made adding a quaternion motion based on the angular velocities with noise to each one of the sigma points.

APPENDIX D BINGHAM-GAUSS
Is possible to use the definition of conditional probability to construct a distribution that consists in the product of a Bi distribution and a Gaussian distribution conditioned on the Bi distributed random variables. The BiGa PDF is given by [7]: P BG q, ω; M, Z, m ω , P q , P ω , P qω = P G (ω; m ω + P T qω P −1 q q, P ω − P T qω P −1 q P qω )P B (q; M, Z) where M and Z are the orientation matrix and the matrix of concentration parameters of the Bi part defined by P q (46) and P G is a Gaussian PDF given by: with mean µ = m ω + P T qω P −1 q q and variance σ 2 = P ω − P T qω P −1 q P qω as shown in (58).

A. DISTRIBUTION PARAMETERS
The parameters m ω , P ω , P q and P qω (58) are given by:

B. ANTIPODAL SYMMETRY
We have to guarantee that this distribution is antipodally symmetric with q and −q representing the same attitude. The PDF described in (58) needs to be divided into two hemispheres of the unit hypersphere to guarantee that condition [7]. The BiGa PDF becomes represented as: P BG = P BG q, ω; m ω , P ω , P q , P qω q ∈ S + P BG q, ω; m ω , P ω , P q , −P qω q ∈ S − (64) where S + and S − represent the hemispheres. For each q its position on the hypersphere is obtained analyzing its last nonzero element. If it is negative the quaternion belongs to S − otherwise belongs to S + .

APPENDIX E BINGHAM-GAUSS FILTER SIGMA POINTS CREATION
We follow the same approach to create the canonical sigma points as described in Appendix C for the Bi case but adding a Euclidean part describing the angular velocity. The canonical sigma points that represent the deviation for the Euclidean part are defined as [10]: The angular deviations in the first three states of the attitude quaternion are introduced while the Euclidean part is held constant at zero to guarantee that the perturbed quaternion remains on the unit hypersphere according to [10]: 8 [± sin α 1 , 0, 0, cos α 1 ] T ,ω The parameters α i and δ are given by: The weights for the sigma points one to six are given by: where r and s are equal to three in this case, N is equal to thirteen (the number of sigma points) and f s+1 is obtained analyzing the second moment of the canonical BiGa distribution (the zeroth and first moment is one and zero respectively) according to: where the covariance can be obtained as described in (45) for the Bi part of the BiGa distribution alone. The weights for the sigma points seven to twelve are given by: (75) (77) VOLUME 8, 2020 The weight for the central sigma point is given by: Each sigma point is transformed from the canonical representation using the following relations [7]: ω = P ω + P T qω P −1 q P qω Z + P T qω P −1 q M p + m ω q ∈ S s+ (80) ω = P ω + P T qω P −1 q P qω Z − P T qω P −1 q M p + m ω q ∈ S s− where p corresponds to the quaternion part and Z correspond to the Euclidean part of the created canonical sigma points z originating the Z sigma points. This transformation is similar to what is performed in the Bi case but now taking into account the Euclidean part of the sigma point vector as seen in (80) and (81). The parameters m ω , P ω , P q and P qω as described in (60) to (63) can be obtained from the sigma points according to: where Z i is the sigma point i, f ω is the angular velocity part of the considered sigma point and f q is the quaternion part of the considered sigma point. He is currently a Full Professor with the Portuguese Naval Academy, Alfeite, Portugal, and an invited Full Professor with the Information Management School, Nova University, Lisbon, Portugal. He has participated in several national and international research projects as a Principal Investigator and Technical Manager. He has graduated four Ph.D. students and more than 60 M.Sc. students. He published more than 100 research papers in peer-reviewed journals and peer-reviewed conferences. His primary research interests focus on data mining, machine learning, and advanced robotics.
ALEXANDRE BERNARDINO received the Ph.D. degree in 2004. He is currently an Associate Professor with the Department of Electrical and Computer Engineering and a Senior Researcher with the Computer and Robot Vision Laboratory, Institute for Systems and Robotics, Instituto Superior Técnico (IST), and the faculty of engineering at Lisbon University. He has participated in several national and international research projects as a Principal Investigator and Technical Manager. He has graduated 12 Ph.D. students and more than 80 M.Sc. students. He published more than 40 research articles in peer-reviewed journals and more than 100 papers on peer-reviewed conferences in the field of robotics, vision, and cognitive systems. His primary research interests focus on the application of computer vision, machine learning, cognitive science, and control theory to advanced robotics and automation systems. He was a Co-Supervisor of the Ph.D. Thesis that received the IBM Prize 2014 and the Supervisor of the Best Robotics Portuguese M.Sc. Thesis Award of 2012. He is currently the Chair of the IEEE Portugal Robotics and Automation Chapter. He is an Associate Editor of the journal Frontiers in Robotics and AI and major robotics conferences (ICRA and IROS). VOLUME 8, 2020