Gyro-Free Kalman Filter with Unknown Inputs for SO(3)-based Attitude Estimation

Rotational motion (or attitude) estimation algorithms in navigation rely mainly on measurements from a set of sensors, including a magnetometer, gyroscope and accelerometer, also known as magnetic, angular rate, and gravity (MARG) sensor array. However, the significant power consumption of the gyroscope and its intrinsic bias motivate the need for more suitable solutions. In this paper, a robust two-stage Kalman filter is designed for attitude estimation in the special orthogonal group SO(3), by considering the angular velocity as unknown input. The performance of the proposed algorithm was evaluated through Monte Carlo simulations compared to the known TRIAD algorithm, which utilizes measurements from only accelerometer and magnetometer sensors, and to the Invariant Extended Kalman Filter (IEKF), which is applied for SO(3) estimation using MARG sensor array.


I. INTRODUCTION
In the last several decades, navigation technology advanced significantly, playing a crucial role in several domains, such as military, missile technology, space operations, and indoor positioning.It has continued to evolve and is now widely used in smart devices.To facilitate navigation, considerable progress has been made in the technology, as well as in the theory and algorithms of data fusion.Two main categories of algorithms exist to estimate the attitude, one of the navigation states, of a rigid body: static and dynamic.On the one hand, static algorithms utilize only observations from accelerometer and magnetometer sensors, without taking into account the history of measurements or the body's dynamics, and include some known algorithms such as FQA [1], TRIAD [2], and QUEST [2].On the other hand, dynamic algorithms use a dynamic model with angular velocity from a gyroscope as an input to predict the attitude, while measurements from the accelerometer and magnetometer are used to update the prediction.The attitude dynamics, which can be expressed using either quaternion, rotation matrices, or Euler angles, are nonlinear and pose several challenges.The utilization of Euler angles representation introduces the issue of singularity [3].Using quaternion or rotation matrix dynamics with the Extended Kalman filter (EKF), the commonly used algorithm for attitude estimation, leads to an estimation error without physical meaning, affecting the filter update step.Additionally, a normalization of the state, represented by quaternion for example, is required at each time step, which can further affect This work is partly supported by the project TRIP-H2 (CNRS).
the quality of estimation [3].As a result, recent works in the literature were developed using the Invariant Extended Kalman filter (IEKF) theory, with attitude dynamics expressed in the special orthogonal group SO (3), which eliminates the need for normalization and allows the error to be expressed in R 3  vector space.This error can be mapped to an element in SO(3) using the function called "exponential map" [4], [5].
In addition to advancements in theory, significant progress has also been made in technology.In the past, large, expensive, and power-consuming gyroscopes were developed.However, with the introduction of low-cost Micro-electro-mechanical Systems (MEMS), the size and cost of gyroscopes have significantly reduced.Although MEMS gyroscopes offer advantages, they also have some drawbacks.One of the main drawbacks is that MEMS gyroscopes have a relatively high-power consumption compared to accelerometers and magnetometers, with MEMS accelerometers and magnetometers operating in the microampere range while MEMS gyroscopes typically require a few milliamperes [6]- [8].Such high current consumption is unsuitable for applications requiring low-power consumption, such as smart devices (smartphones, tablets, etc.).Another important drawback of MEMS gyroscopes, especially in lowcost Inertial Measurement Units (IMU), is their susceptibility to drift and bias, [9].Gyroscopes are also prone to noise, which tends to increase over time.Also, in order to use dynamic algorithms such as the IEKF, the covariance matrix of this noise should be known, which can be challenging and may not always be feasible.
To avoid the use of gyroscope, one existing solution consists in applying static attitude estimation algorithms.Additionally, there have been several studies in the literature that concentrate on developing angular motion estimation algorithms, to be able to achieve attitude estimation task without gyroscope.These algorithms are commonly known as gyro-free attitude estimation techniques [7], [10], [11].Such algorithms rely on utilizing several accelerometers placed in specific spatial configurations.However, these works require knowledge of the sensors' exact positions and consider the measurements to be deterministic.To the best of the authors' knowledge, no literature currently addresses the problem of gyro-free attitude determination using attitude dynamic equations and dynamic estimation algorithms.
In this paper, we propose to consider the angular velocity as unknown input and hence avoid the use of gyroscope for attitude estimation purposes.State estimation with unknown inputs has been the subject of numerous theoretical studies in the literature for linear systems when the unknown input affects the state dynamic only [12], [13], or when it may also affect the output [14], [15].Some of these works were extended to nonlinear dynamics [16].In these works, the proposed approaches were evaluated with simple theoretical examples and the problem of attitude estimation with unknown angular velocity input was never addressed.Moreover, even implementing the existing algorithms for nonlinear systems with unknown inputs would have drawbacks of the normalization step in the case of the quaternion.Finally, our aim is to develop an algorithm for attitude estimation in SO(3), with unknown angular velocity input.This angular velocity only affects the state dynamics.The Robust Two-Stage Kalman Filters for systems with unknown inputs (RTSKF) [12], developed for linear systems by making use of a two-stage Kalman filtering technique, is proposed for this estimation problem.In this framework, the attitude dynamics is represented in SO(3) with unknown angular velocity input, i.e. the use of gyroscope will be omitted, and the outputs are measurements from a 3axis accelerometer and a 3-axis magnetometer.The main contributions of this paper can be resumed as follows: • The extension of the RTSKF to include systems represented in SO(3).• The design of a novel algorithm for gyro-free attitude estimation, where the dynamics relating the attitude to the unknown angular velocity input is used.The paper is structured as follows: Section II introduces the mathematical concepts of skew-symmetric matrices and SO(3) group necessary for the estimation purpose.It also explains the mathematical model expressed under this special group and the problem statement.Section III revisits the RTSKF for easy reference and lists the algorithm steps.Section IV presents the proposed RTSKF-SO(3) for SO(3)-based attitude estimation.Section V shows the Monte Carlo simulations and comparison results with two algorithms from the literature, the TRIAD and IEKF algorithms.The paper ends with Section VI where we summarize our findings and discuss potential future research directions in this field.

II. PRELIMINARIES AND PROBLEM STATEMENT A. Skew symmetric matrix
Skew-symmetric matrices are defined as matrices A that meet the criterion: A T = −A.Three-by-three skewsymmetric matrix (ξ) × that is corresponding to a vector ξ ∈ R 3 is written as: We can perform cross-product operations between two vectors in R 3 in the form of matrix multiplications.Consider we conclude with this usful property: Lemma 1.Let u and w be two linearly independent vectors in R 3 .Then, the matrix u × w × has full column rank.
Proof.Let us assume that the matrix u × w × is not a full column rank.This means that there exists a vector f ∈ and w × f = 0, which give us u × f = 0 and w × f = 0, and thus both u and f are linearly dependent, and similarly, both w and f are linearly dependent.Consequently, w and u are linearly dependent.Therefore, the initial assumption that the matrix u × w × does not have a full-column rank must be false.Hence, it has indeed a full-column rank.

B. The orthogonal group SO(3)
The attitude describing the rotational motion of a rigid body in 3D space between two frames, Earth-fixed frame and body frame, can be represented using rotation matrices that belong to the orthogonal group SO [17].The exponential map exp m : R 3 → SO(3) allows transferring element ξ ∈ R 3 to the SO(3) accordingly to the following formula [17]: Some key properties of the exponential map [4]: First-order approximation of (3) when ∥ξ∥ ≪ 1 gives: For small v 1 , v 2 ∈ R 3 (i.e∥v 1 ∥ ≪ 1, ∥v 2 ∥ ≪ 1), we also have this approximation [5]:

C. Attitude dynamics represented on SO(3)
The discrete-time rotation dynamics of a rigid body can be expressed as [18], [19] : where the subscript k denotes the time step number, R k ∈ R 3×3 is the rotation matrix from the body frame to the Earthfixed frame, where ω true k ∈ R 3 is the true angular velocity and ∆T is the sampling time, w k is the process noise which is assumed to be mutually uncorrelated zero-mean, white random signal with covariance matrix Q k .The measurement equation, expressed as Y k ∈ R 6 , is composed of measurements from both 3-axis accelerometer and 3-axis magnetometer.In the absence of external accelerations (linear accelerations due to motion) and magnetic disturbances, the only acceleration that will be measured is the one related to the Earth's gravity.Similarly, the only magnetic field that will be measured is the Earth's magnetic field.Then Y k is given by: where a b k ∈ R 3 , B b k ∈ R 3 are the accelerometer and magnetometer measurements written in the body frame, respectively.g ∈ R 3 , m e ∈ R 3 are the Earth's gravity acceleration and the Earth's magnetic field written in the Earth's fixed frame, k represents the measurement noise vector, which is assumed to be mutually uncorrelated zero-mean, white random signal with covariance matrix R k .Also, the measurement function is defined by h : R → R −1 g R −1 m e , then Y k could be re-written as:

D. Problem statement
Equations ( 8) and ( 10) define the discrete-time system model: Our aim is to design an estimator for (R k ) k∈N * when the input (ω k ) k∈N is unknown using only the measurements (Y k ) k∈N , and by exploiting the RTSKF [12] for systems with unknown inputs.For this purpose, we revisit the RTSKF for linear systems in Section III and then design the new algorithm, which we call RTSKF-SO(3) in Section IV, which extends the RTSKF to the case where the dynamics are formulated in the special orthogonal group SO(3) as in (8).Utilizing SO(3) as the attitude representation offers advantages over alternative representations like quaternions or Euler angles.

III. OVERVIEW ON THE RTSKF ALGORITHM
For easy reference, the main details of RTSKF for systems with unknown inputs [12] are listed in this section.To begin with, the algorithm considers the linear discrete-time system model as follows: where are the system state, the unknown input, and the system output (measurements), respectively.The matrix E k has the appropriate rank: rank[E k ] = p.w k , v k are the process and measurement noise assumed to be mutually uncorrelated zero-mean, white random signals with covariance matrices of Q k and R k respectively.Certain conditions related to the matrices A k , E k , and H k need to be satisfied to make the RTSKF works properly.These conditions are as follows [12]: The RTSKF works recursively and the steps for time step k are listed in the Algorithm 1, which takes as inputs: the state estimation and state estimation error covariance matrix in the previous time step (x k−1 and P x k−1 respectively) and the output vector in current time step (y k ).Steps 1 and 2 are the time update (prediction), step 3 calculates the innovation (the output prediction error), step 4 calculates the innovation covariance matrix, and step 5 calculates the Kalman gain K x k to correct the predicted state in step 6 in order to minimize the state estimation error covariance matrix Px k in step 7. Steps 8, 9 and 10 estimate the unknown input dk with a minimum covariance matrix P d k .Finally, steps 11, 12 and 13 give the final state estimation xk with the final state estimation error covariance matrix P x k after substituting the estimation of the unknown input.More details exist in the original paper [12].
Algorithm 1 RTSKF for state estimation with unknown inputs The innovation can be written as follows: Equations ( 14) and ( 15) define the estimation error system and will be used in Section IV.

IV. RTSKF FOR ESTIMATION IN SO(3)
This section aims to design the RTSKF with unknown inputs for attitude estimation in SO(3) and demonstrates how it can be applied to the system defined in ( 8) and (10) when angular velocities from a 3-axis gyroscope are unknown.In the time update, we consider zero angular velocity similar to the first step of Algorithm 1, then (8) leads to: To compute the time update state estimation error, the difference R k − Rk|k−1 has no physical meaning, instead we define the time update state estimation error η k|k−1 and the state estimation error η k as follows: Both η k|k−1 and η k belong to SO(3), so they can be mapped with ξ k|k−1 , ξ k−1 ∈ R 3 using the exponential map exp m (.) as defined in (3): By using ( 16) and ( 18) we get: Employing the model ( 8) we get: Applying the approximation (7) gives: The innovation can be written such as: By substituting R k with (20), it follows successively: Using the first-order approximation (6), we obtain: Finally, using the useful property (2), we obtain: Equations ( 21) and ( 22) are the linearized estimation error system in R 3 akin to ( 14) and (15).The linearized estimation error system has the matrices: Hence the conventional RTSKF with unknown input can be applied to derive the gains K x k , K d k , V k and the updated covariances (after verifying that conditions ( 13) are satisfied).The terms K x k ỹk and V k dk in steps 6 and 12 are corrective shift computed on the linearized error system.Therefore, exponential mapping of the corrective shift is applied to correct the rotation matrix [4], [5].This step should be applied twice for both steps, 6 and 12, respectively, as follows: Let us now check if the matrices in (23) satisfy the conditions (13), the vectors R−1 k|k−1 g and R−1 k|k−1 m e represent the Earth's gravity and Earth magnetic field vectors written in the same frame respectively, then they are linearly independent, then by using Lemma 1 we conclude that rank[H l k ] = 3, and also we have rank Then the conditions (13) are satisfied.It is important to note that matrices A l k and E l k are independent of the estimation, but the matrix H l k is not.However, an approximation is used to obtain the innovation covariance matrix Cov( Ỹk ) = H l k Cov(ξ k|k−1 )H l T k + R k (the same approximation is used for the derivation of the EKF algorithm [20]).Finally, Algorithm 2 lists the RTSKF for SO(3) estimation.To evaluate the proposed RTSKF-SO(3), we implemented three algorithms: RTSKF-SO(3), TRIAD [2], and IEKF [5].Table I summarizes the implemented algorithms, including the sensors utilized and their corresponding current consumption range.To ensure a fair comparison with IEKF, we conducted multiple scenarios with varying levels of gyroscope noise.For each comparison/scenario, we performed Monte Carlo simulations with 100 iterations to obtain reliable results.

A. Setup of the simulation
The system was simulated for 100 s for each Monte Carlo iteration with a sampling time of ∆T = 0.01 s (100 Hz).The NED (North-East-Down) frame was chosen as the fixed one, where the approximate values for the Earth's gravity and magnetic field in France are g = [0, 0, 9.81] T m/s 2 and m e = [0.23,0.01, 0.41] T Gauss, respectively.To obtain the ground truth of the rotation between the body and the fixed frames, the true angular velocity for the body was set as shown in Table II.The ground truth of rotation was then calculated using (8) with an initial rotation matrix corresponding to [45 • , 45 • , 45 • ] in terms of Euler angles (Roll, Pitch, Yaw) using XYZ convention (Section 3.3 in [3]), and the true acceleration and magnetic field written in the body frame were generated using (9) with zero noises.Subsequently, noisy measurements were generated by adding white noise with zero mean and chosen standard deviations to the true angular velocity, acceleration and magnetic field (the latest two written in the body frame) to obtain the gyroscope, accelerometer, and magnetometer measurements along three axes, respectively.The standard deviations for the accelerometer and magnetometer noises were set to be σ a = 0.01 m/s 2 and σ m = 0.005 Gauss, respectively.Several levels of gyroscope noise were performed when using IEKF.Note that changing the gyroscope noise level does not affect the results of our proposed approach, as it does not depend on gyroscope measurements.In order to initialize the three algorithms (particularly RTSKF-SO(3) and IEKF), the initial estimated rotation matrices and initial estimation error covariance matrices were set to be identities.

B. Results and discussions
To compare the performance of the proposed RTSKF-SO(3) with the TRIAD and IEKF algorithms, it is necessary to express the estimation error using easily interpretable information.To achieve this, all rotation matrices were converted to Euler angles using the XYZ convention.The Root Mean Square Error (RMSE) was then computed for the three algorithms for the time range [2,100]s, where we start computing the RMSE after 2 s to guarantee the convergence of RTSKF-SO(3) and IEKF.The estimation error was computed relative to the ground truth rotation, which was also converted to Euler angles.The RMSE was computed for each Monte Carlo iteration, and in the end, the average was taken for the 100 iterations.The results will be presented in three parts: first, a figure showing the effectiveness of the estimation for the proposed algorithm; second, a comparison with TRIAD and third, a comparison with IEKF.
1) RTSKF-SO3 estimation results: Figure 1 shows the estimation error obtained with RTSKF-SO(3) for Euler angles, demonstrating the accuracy of the estimation.The left part of the figure represents the estimation error within the time range of [0, 0.2] s, while the right part corresponds to the time range of [0.2, 100] s.In the left part, it is clear that RTSKF-SO(3) achieves convergence quickly, specifically in less than 0.05 s.In the right part, we can observe that the roll error mostly stays between −0.5 and 0.5 deg.The pitch error remains between −0.2 and 0.2 deg, and the yaw error is between −2 and 2 deg.The yaw angle error is larger because the yaw estimation relies solely on magnetometer measurements since gravity, which has a vertical direction, plays no role in determining yaw.These findings indicate that the RTSKF-SO(3) estimation method performs well and provides good results even if the angular velocity is unknown.2) Comparison between RTSKF-SO(3) and TRIAD: Table III presents the RMSE (in degrees) for both RTSKF-SO(3) and TRIAD.The proposed algorithm RTSKF-SO(3) shows a 20% improvement in RMSE compared to TRIAD using only accelerometer and magnetometer measurements.Additionally, it is important to mention that further enhancements can be made to RTSKF-SO(3) to account for scenarios where the body experiences linear acceleration or magnetic disturbances.
One potential improvement could involve adapting the measurement covariance matrix [21], which is not feasible when using static algorithms like TRIAD.3) Comparison between RTSKF-SO(3) and IEKF: In the Table IV, we calculate the RMSE (in degrees) for both RTSKF-SO(3) and IEKF for various gyroscope noise levels (various gyroscope noise standard deviations σ ω ).The obtained results indicate that RTSKF-SO(3) has comparable accuracy with IEKF, even if it does not use measurements from a 3-axis gyroscope.Sometimes RTSKF-SO(3) can perform better, and sometimes not, depending on the gyroscope noise standard deviation.In all cases, the difference in RMSE between the two algorithms remains small.Further, this simulation helps us to note the following challenges as motivating factors for using RTSKF-SO(3): • the IEKF remains sensitive to the gyroscope noise and, therefore, to the quality of the sensor, while RTSKF-SO(3) does not, • in experimental tests, higher power consumption will be observed with IEKF than RTSKF-SO(3).By using RTSKF-SO(3), the current consumption would decrease from milliampere to microampere range with a slight decrease in the accuracy, • the need to know the gyroscope noise covariance matrix and bias with IEKF but not with RTSKF-SO(3).

VI. CONCLUSION AND FUTURE WORK
This paper proposed a dynamic algorithm for attitude estimation using measurements from a 3-axis accelerometer and a 3-axis magnetometer while considering the angular velocity as an unknown input.The proposed RTSKF-SO(3) was evaluated using Monte Carlo simulations, and the results show that it outperformed TRIAD, which is a commonly used static algorithm for attitude estimation.Moreover, RTSKF-SO(3) demonstrates better performance than the Invariant Extended Kalman filter (IEKF) in high gyroscope noise scenarios.Therefore, the proposed algorithm can provide an effective solution for attitude estimation in situations where gyroscope measurements are not reliable or unavailable.In future works, it is important to validate the results of the proposed RTSKF-SO(3) algorithm on real data and compare the power consumption of IEKF and RTSKF-SO(3) in real applications.Another direction for future works is the extension of the proposed algorithm to handle the case when the body is subject to linear acceleration.

k 14 :
return xk , P x k Let us define the time update estimation error xk|k−1 = x k − xk|k−1 and the estimation error xk = x k − xk .By applying (11) and step 1 of Algorithm 1 in the time update estimation error equation, we get:

k 14 :
return Rk , P x k Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.2023 13th International Conference on Indoor Positioning and Indoor Navigation (IPIN) V. SIMULATIONS OF THE RTSKF-SO(3) ESTIMATION: SETUP AND MAIN RESULTS

TABLE II :
The true angular velocity used in simulation

TABLE III :
RMSE (in degrees) for both RTSKF-SO(3) and TRIAD

TABLE IV :
RMSE (in degrees) for both RTSKF-SO(3) and IEKF for various gyroscope noise level