Evaluating Tracking Rotations using Maximal Entropy Distributions for Smartphone Applications

Recursive attitude estimation of a rigid body from inertial measurements is a crucial component of many modern systems and as such, has a rich historical background of proposed techniques. Recent work has been done on tracking rotations using maximal entropy distributions. However, there has been no evaluation done on the performance of this approach using real inertial data. In this work, we investigate the performance and limitations of classical and modern probabilistic Bayesian approaches and provide a rigorous comparison to attitude estimation on the special rotation group SO(3) using maximal entropy distributions. The extended Kalman Filter and the unscented Kalman filter are derived as benchmarks in attitude estimation from low-cost inertial measurement units, commonly found in smartphones. To evaluate robustness over multiple sampling intervals, we generated synthetic directional inertial measurements from a typical low-cost 3-axes inertial measurement unit and use the Frobenius Norm as our primary metric. To further our evaluation, we took advantage of a publicly available dataset where inertial measurements are recorded from a number of off-the-shelf smartphones and the ground truth is calculated using a Motion Capture system. Our experiments demonstrate that tracking rotations using maximum entropy distributions produce a more accurate and robust solution in contrast to alternate proven Kalman approaches.


I. INTRODUCTION
T HE ability to accurately and recursively estimate the attitude of a rigid body is a key component in many applications, including aerial, autonomous and subaquatic navigation [1], satellite control and space junk estimation [2], [3], as well as augmented reality and tracking of human and animal body motions [4]. Due to its vast range of applications, it has been the subject of extensive research. Over the years, numerous classic nonlinear estimation algorithms have been put forward to address dynamic state estimation in a nonlinear system and an exhaustive summary of these approaches is given in [5]. Many of the techniques described are computationally complex and particular to a certain device or application. The major issue with nonlinear filters is real time computational complexity to provide a given estimation accuracy. Real time computational complexity depends on several factors, which are explained in [6]. In this work we look at algorithms that are on the order of d 3 for estimating state vectors of dimension d. Additionally, we look at Bayesian estimators, parameterised by rotation matrices that derive attitude perturbation states in the filter. This is due to Bayesian estimators being advantageous in their complete stochastic property definition given by the probability density of the estimated attitude, particularly the level of confidence [7].
Attitude estimation algorithms typically rely on directional measurements acquired, for example, from an inertial measurement unit (IMU). Advancements in microelectromechanical technologies have allowed for enough processing power in present-day mobile and tablet devices to accommodate an increasing number of sensors. Consumer grade IMUs availability, low-cost, and low power requirements enable such devices to facilitate real-time applications and navigation solutions [8], [9].
In this paper we focus on Bayesian estimation algorithms that use tri-axial measurements from 3 inertial sensors, commonly found in smartphones. These embedded sensors make it possible to leverage the continuously provided information to estimate the attitude of the rigid sensor body with respect to the Earth's local frame. These IMUs typically consist of a tri-axis accelerometer, gyroscope and magnetometer. The magnetometer and accelerometer provide noisy directional measurements with respect to the Earth's polar North and gravitational acceleration. The gyroscope provides noisy measurements of angular velocity. Directional vector observations can be taken from accelerometer and magnetometer, whereas the gyroscope provides angular velocities. Integration of the angular velocity measurements unfortunately leads to increasingly large errors in attitude estimation due to the sensor bias. A more in depth description of these sensors and their calibrations is provided in [8]. The modelling of the accelerometers, rate gyroscopes and magnetometers take into account the several biases and noise imperfections found in smartphones. An in depth look into a wide range of IMUs and their deficiencies can be found in [10].
As the integration of gyroscope measurements yields poor estimations, accelerometer and magnetometer measurements are used to update error calculations and compensate for the drift. The generalised problem for attitude estimation from IMUs is in the combination of these sensors to provide an optimal solution in the form of an optimal state estimator. Optimal state estimators have been proposed where measurements are mixed with both kinematic and dynamic models [5], [8], [9]. Most of the complexity in attitude estimation lies in the nonlinearity of attitude change and as such requires a non linear estimator. Previous work on attitude estimation with smartphones in [8] and [9], has provided a number of state-of-the-art nonlinear estimators in this space that fit our computational complexity requirements.
The non linear state estimators looked at in this work are: the extended Kalman filter (EKF), the unscented Kalman filter (UKF) and tracking attitude using maximum entropy distributions on the rotation group SO(3), developed by Suvorova et al. [11] -henceforth referred to as the SO (3) filter. We generated synthetic IMU measurements at varying sampling rates and utilise the publicly available OXIOD smartphone dataset [12] to thoroughly compare and contrast each estimation solution. Improving our state estimation from consumer IMUs could promote significant advances in practical applications such as indoor navigation and localisation, inertial odometry and augmented/virtual reality, amongst others. The major contribution of this paper is to provide a rigorous comparative evaluation of attitude estimation algorithms in literature in contrast to tracking rotations using maximal entropy distributions in terms of robustness and accuracy.
The rest of the paper is arranged as follows: the background fundamentals of attitude estimation from IMU data via an EKF, UKF or SO(3) filter used in this work is given in Section II. Detailed mathematical descriptions are given in Section III, IV and V for the respective filters. The experimental results are presented in Section VI alongside a discussion. Finally, we draw some conclusions and delineate potential future work in Section VII.

II. BACKGROUND
A Kalman-based filter has long been the de facto standard for attitude estimation algorithms and their commercial applications [5]. The extended Kalman filter (EKF) was the pioneering real-time attitude estimator and its inception proved integral in live spacecraft attitude estimation [2], [3]. The pervasiveness of the EKF solution is a testament to its effectiveness, however, there are a number of disadvantages.
To account for the nonlinear aspects of rotational kinematics, the EKF predicts the states of the system under the assumption that its observation model and process model are locally linear and then expands upon these using Jacobians. Since the rotation group on SO(3) has three dimensions, most attitude determining EKFs use lower-dimensional parameterisations [5], [13]. The low dimensionality can lead to implementation difficulty when expanding the Jacobians, as certain attitudes are singular or discontinuous. These singularities cause the EKF error covariance matrix to shrink rapidly in size. Once this matrix is sufficiently small, the EKF has to be re-initialised, causing the estimations to drift over time [5], [3], [14]. Additionally, the calculation of the Jacobian matrix in an EKF is a cumbersome process and is not guaranteed to exist or may not have a finite value [15]. Quaternion based filters have been developed to overcome some deficiencies [2], [16], as quaternion based parameterisations do not exhibit singularities in representing attitude.
Several extensions have been built on the framework of the EKF -most notably the unscented Kalman filter. The UKF is an improvement to the EKF and it generalises the Kalman filter for both linear and nonlinear systems [17]. The UKF builds around the premise that with a fixed number of parameters, approximating a Gaussian distribution should be far simpler than approximating some arbitrary nonlinear function. Introduced in [18], the UKF has been implemented for attitude estimation in [14], [19] with much success. It has several advantages over the EKF, namely a lower expected error as it avoids the derivation of Jacobians as well as being valid for higher order expansions. The UKF uses sigma points to capture the characteristics of a Gaussian distribution. However, the regenerative step of the sigma points can be a major limitation in the propagation of state uncertainty [20].
Alternate estimations for the probabilistic distribution of attitude uncertainty on the manifold have been studied, VOLUME 4, 2016 despite being relatively unpopular in comparison to the aforementioned techniques [7]. Work on attitude estimation on the special orthogonal group is seen in [7], [11], [21], [22], [23], [24]. In this body of work, the specific form of probability density, the matrix von Mises-Fisher (vMF) distribution, is used to represent the uncertainties in the state estimates. The vMF distribution is defined by 9 parameters when applied to the special orthogonal group SO(3) and, as such, is comparable to the Gaussian distribution in R 3 , which is completely represented by the three dimensional mean and six dimensional covariance (used in our Kalman variants). We are grateful for Dr Sofia Suvorova's early discussions on the background for this work. Following [11], [23], we use the maximal entropy distributions on the rotation group -the von Mises-Fisher distributions -as a model for the priors in tracking rotations in the SO(3) filter. To the best of our knowledge, tracking rotations from real inertial measurements in this manner, is absent in literature. This work presents a novel evaluation of tracking rotations using maximum entropy distributions, from real and synthetic inertial measurements. We leverage popular Kalman filters as benchmarks.

III. SO(3) FILTER
The group of rotations in three dimensions is the special orthogonal group SO(3). Attitude of an object in three dimensional space can be represented by an element of the special orthogonal group that corresponds to its rotation from some arbitrary initial state. The vMF distribution is the maximal entropy probability over these rotations. Any attitude estimation approach starts with the definition of the state vector that defines the time varying system and then to propagate this state by computing the posterior distribution of the rotating body.

A. MEAN AND CONCENTRATION MATRICES ON SO(3)
The state of the system is represented by a rotation matrix, an element of the special orthogonal group SO(3), where where I 3 is 3-dimensional identity matrix. The matrix von-Mises Fisher distribution on SO(3) is defined as follows.
Definition. A random rotation matrix, R ∈ SO(3), is distributed according to a vMF distribution if its probability density function is defined on SO(3) as where etr (·) = exp tr (·) , matrix A ∈ R 3×3 , encompasses said location (rotational mean) and measure of spread (concentration) and α(A) ∈ R is the normalization factor with respect to the Haar measure on Lie group SO(3). Furthermore, we denote R ∼ vMF(A).
Proposition. The product of two matrix vMF distributions can be written into a matrix vMF distribution after normalization, i.e.
The matrix A can be decomposed as the product A = R T A 0 =R T VΣV T , see [25], whereR is the polar component and A 0 = VΣV T is the elliptical component, also known as the concentration matrix. Equivalently, we have R ∼ vMF(R T A 0 ). It is important to note that if A is singular, this decomposition may not be unique. Function α(A) can be calculated in terms of an asymmetric version of the hyper-geometric function 0 F 1 where Σ is a matrix of eigenvalues of the concentration matrix.
The first moment is given by where f (Σ) is a diagonal matrix with entries f i defined by A closed-form and approximate method to calculate f (Σ) can be found in [23].

B. LIKELIHOOD FUNCTION AND PRIOR DENSITY
The Bayesian framework proposed in [23] assumes that the posterior density of the rotation matrix at time k, i.e. R k , is vMF distributed: where Z i is the measurement at time i, A k is concentration matrix andR T k is a mean. Obviously, the A k andR T k should be dependent on Z k .
For the state transition, it is assumed that, at time k, the attitude R k is transited from R k−1 through another random The state dynamics transition probability is then written in terms of the distribution matrix, P k The prior distribution of R k is then given by This resultant integral is intractable and is therefore approximated with a vMF distribution to allow for a recursive process to estimate the attitude over time. Since R k is assumed to be distributed by a vMF, then its first moment can be estimated as the product of two vMF variates, Eq. (5) where Equivalently, (11) can be polar decomposed tô so we then have the distribution, R k , approximated by where Q k is the mean, To summarise, in order to find the vMF distribution R ∼ vMF(R|R T A) approximating the distribution of a rotation matrix R w.r.t Z, we first compute the polar decomposition of Z to obtain the modal matrixR.

C. MEASUREMENT MODEL AND BAYESIAN UPDATE STEP
The measurements, Z k , are assumed to be distributed in accordance with a vMF. As such, the likelihood function at time k is where the matrix X is columned by the gravity and magnetic reference vectors (in a global coordinate system) and M is a matrix with diagonals that correspond to the measurement concentration parameters. From Proposition III-A, we know that the posterior distribution is still vMF distributed. Specifically, recall the proposed form of the posterior distribution, we have following result via Bayes's rule where the new parametersR k and A k are computed as the polar decomposition of matrix XMZ T k + Q T k C k -as done in Eq. (5).

IV. EXTENDED KALMAN FILTER
An EKF was used to estimate the attitude alongside the SO(3) Filter. In this section we outline how the filter was built [26], [16], [5], [3]. The rotation of an object in motion can be defined by Euler angles, represented by roll φ, pitch θ and yaw ψ. The angular rate is defined by ω x , ω y and ω z along x, y and z axis respectively. We then have where and s x = sin(x), c x = cos(x).
Now to construct Kalman filter, the following model is considered where where T k = T(v k ) and u k is the gyro measurement. The transition model is where g(·) is the nonlinear function defined in (19) and (20) and w k is the noise term with mean zero and covariance Q.

VOLUME 4, 2016
The measurement model is where h(·) is the nonlinear function, R T k is rotation matrix, r a and r m are nominal gravitational acceleration and magnetic field vector, and v k+1 is Gaussian noise term with mean zero and covariance C.
The EKF is therefore given bŷ where Fig. 1. Due to the linearization process of the EKF (left), A k−1 x k−1 is on the tangent plane of point x k−1 and has an error with desired g(x k−1 , u k−1 ), where g(x k−1 , u k−1 ) is transition model and A k−1 is linearization of function g. Whereas in the SO(3) tracker (right), the local linearization process is not involved and the distribution is located directly on the unit-sphere.

V. UNSCENTED KALMAN FILTER
The unscented Kalman filter is a powerful tool when the underlying models are nonlinear. Based on the work in [27], we complete the UKF formulation here. The quaternion based UKF can be found in [28].
Then the weights are The prediction step and measurement update step are given as follows 29) where P xy,·|· is covariance matrix of x ·|· and y ·|· . Finally, the correction step is

VI. EXPERIMENTS
In this section we illustrate the approaches presented in the previous sections with synthetic and real IMU measurements. The simulated measurements were generated to best mimic a typical IMU found in a modern smartphone, where white gaussian measurement noise standard deviations were given by 0.01G, 0.1 rad/s and 0.4µT for each axis of the accelerometer, gyroscope and magnetometer, respectively [29]. Doing this synthetically gives us access to the ground truth over the exact same rotations with the ability to vary our sampling rate. Our experiments involve 1000, 10 second long, Monte-Carlo simulations over multiple sampling rates. The OXIOD dataset is used to evaluate each estimation solution on a large set of real world data, comprising of multiple smartphones, users and activities. The combination of experiments were designed to best demonstrate each algorithm's robustness and applicability.

A. ATTITUDE ESTIMATION: SYNTHETIC DATA
The most valuable performance metric in the proposed attitude estimation systems is the estimation error while the sensor is in dynamic motion, where there is no rest time for the filters to reinitialise without compromising the current estimate. We have simulated measurements from IMUs, comprising of a triaxial accelerometer, gyroscope and magnetometer. The simulation was implemented with  [28], and the sampling rate at 10Hz, 100Hz and 1000Hz. The simulation results are shown in Fig. 2 -10. The Frobenius norm of the differences between reference and estimate are given, as follows where t denotes the time,R(t) is the estimates of the attitude at time t and R is rotation matrix of the reference attitude. These are displayed in Fig. 4, 7 and 10 and provide a visually simple, 3-dimensional performance depiction where L(t) corresponds to the closeness between the attitude estimation and truth.    Figure 4, it is evident that the process model of the SO(3) tracker, defined on the sphere directly using a vMF distribution, is advantageous when compared to Kalman process models which are defined on the tangent plane. This is due to the position of the Normal distribution in Euclidean space. As a result, when the time between samples increases, the Kalman filters will introduce additional uncertainty due to the linearization process, when compared to the SO(3) tracker. The Frobenius norm values for the SO(3) tracker (from Equation (31)) are small throughout and do not accumulate over time. From the progression of Figures 4 to 7 and then 10, it is clear that the performance of EKF and UKF becomes more and more comparable with the SO(3) as the sampling rate increases. This coincides with the theoretical property of the Kalman filters, in which the local linearization improves due to the shorter intervals between measurements. Additionally, we can see that the SO(3)'s performance is improved accordingly. Our simulations at 10Hz exemplify the difference in the approaches in attitude estimation using low-cost sensors. Best visualised in Fig. 2 and Fig. 4, the EKF struggles in the initialisation and starts drifting almost immediately. In contrast, we see that both the UKF and SO(3) remain constant throughout. However the SO(3) converges much more quickly and provides a more accurate estimation. In summary, when our measurement frequency is f = 1000Hz, the performance of all trackers are reasonably close to each other but when f = 10Hz, the performance of SO(3) is a significantly better solution. It is important to note that despite each of the Kalman approaches convergence to a more comparable solution as the sampling time increases, the SO(3) still remains superior.

B. ATTITUDE ESTIMATION: REAL DATA
The OXIOD dataset is a publicly available at [12] and aims to reproduce everyday activities using off-the-shelf consumer phones and Motion Capture as a means of ground truth. The dataset contains 158 sequences totalling more than 42km in total distance. The data collection was designed to best represent the varying complexity of motions of phone-based IMUs in everyday usage, utilising four different off-the-shelf consumer phones and five different users. The scenarios in the dataset describe the user over three different motion modes: slow walking, walking at a regular pace and running -all with phone in hand. Further scenarios have the user walking at a regular pace with the phone in a handbag, pocket or trolley. We have compared each approach over each scenario, using error variance as our performance metric, for each Euler angle. The variance is given by: where α k is the estimate error at point k, α is the mean error and n is the number of estimates. By leveraging the true attitude representations using the genetic algorithm [30], we were able to calculate optimal covariance parameters for each filter to evaluate their situational best-case solution. Our results are presented in  In Figure 11, the variance for the Euler pitch estimate is given for each approach and activity. Given the results in Section VI-A, it would follow that the SO(3) and UKF produce comparable solutions and the EKF trails with a larger variance. It can be seen that this is largely the case, with an exception when the phone was placed in the pocket of the user. This is possibly due to the EKF having the chance to reinitialise when the device aligned with the gravitational vector. The variance of our roll estimates (in Figure 12) are even more in-line with our theoretical expectations and simulated results, where we see the SO(3) perform best, followed closely by the UKF and then the EKF. Again, in Figure 13, the yaw estimates mirror those from our Monte Carlo simulations, in which the SO(3) tracker significantly outperforms the Kalman variants.
We can numerically quantify the robustness of each approach by looking at the mean variance over the entire dataset. The mean variance of the pitch estimates for the EKF, UKF and SO(3) are 0.38, 0.38, 0.37 respectively. Roll  Since battery saving is a crucial component in a smartphone, processing time is paramount to its applicability. Table 1 reports the number of rotation matrices generated per second using Matlab. The larger the number, the better the performance of the algorithm. Computationally, the EKF outperforms the SO(3) and UKF. The second line indicates the ratio of the time spent in each algorithm compared to the EKF. Overall, in our experimental setting, we observe that each algorithm can be executed on smartphones at 100Hz. It is also of special interest to note that in cases where battery life is of particular importance, the ability of the SO(3) to sample less frequently than the Kalman algorithms without sacrificing much accuracy could allow for further power reductions. However this will need to be further investigated.

VII. CONCLUSION
In this work, we have evaluated popular attitude estimation algorithms: the traditional Extended Kalman filter and Unscented Kalman filter and using maximal entropy distributions on the rotation group SO(3). We tested their performance using simulated IMU measurements, emulated from a typical smartphone over varying frequencies, and a large, publicly available dataset. We showed that the SO(3) outperforms the EKF and UKF estimations under most test conditions, with the largest discrepancies coming from low rate sensor data and yaw estimation. It is well known that the EKF is a sub-optimal and biased estimator due to the errors introduced in the linearization and the resultant calculation of the Jacobian. We showed that both Kalman variants are more sensitive to the change of sampling rate than the SO(3) tracker, due to the different modelling methods. Our evaluations on the real dataset prove the SO(3) filter is not only a better estimate solution, but also more robust. Adoption of the SO(3) filter to process inertial measurements for attitude estimation in smartphones could improve not only attitude reliant applications such as augmented/virtual reality and navigation, but also improve battery life due to its ability to produce accurate estimations at reduced sampling intervals. We look to improve on this solution and future work will aim to optimise the mean and covariance estimates in the SO(3) filter, as well as reformulating gyroscope bias estimation with sigma point or particle estimation.