MARG Sensor-Based Attitude Estimation on SO(3) Under Unknown External Acceleration

In many applications, attitude estimation algorithms rely mainly on magnetic and inertial measurements from MARG sensors (consisting of a magnetometer, a gyroscope, and an accelerometer). One of the main challenges facing these algorithms is that the accelerometer measures both gravity and an unknown external acceleration, while these algorithms assume that the accelerometer measures only the gravity. In this letter, an attitude estimation algorithm on the special orthogonal group SO(3) is designed, considering the external acceleration as an unknown input with direct feedthrough to the output, with a local approximation approach. The proposed algorithm is validated through Monte Carlo simulations and real datasets, demonstrating better accuracy and enhanced performance than existing solutions.


I. INTRODUCTION
A TTITUDE estimation of rigid bodies plays an impor- tant role in navigation for many applications, including smartphones, autonomous vehicles, and virtual reality.It relies basically on measurements from a magnetometer to measure the magnetic field, a gyroscope to measure the angular velocity, and an accelerometer to measure the acceleration.The three mentioned sensors form a triad called the MARG (Magnetic, Angular Rate, and Gravity) sensor module.Among a wide range of MARG sensor-based attitude estimation algorithms proposed in the literature, the extended Kalman filter (EKF) is the most used, where the attitude is represented using quaternion [1], [2].An improved algorithm is the invariant extended Kalman filter (IEKF), where the attitude is expressed using the special orthogonal group SO(3) [3], [4], [5].In both EKF and IEKF, the correction step uses the measured acceleration and magnetic field, which are approximated by the projection of gravity and Earth's magnetic field in the body Ghadeer Shaaban, Hassen Fourati, and Christophe Prieur are with Univ.Grenoble Alpes, CNRS, Inria, Grenoble INP, GIPSAlab, 38400 Grenoble, France (e-mail: ghadeer.shaaban@gipsa-lab.fr;hassen.fourati@gipsa-lab.fr;christophe.prieur@gipsa-lab.fr).
One of the main challenges facing attitude estimation algorithms, employing MARG sensor in scenarios involving periods of accelerated motions, is that the accelerometer measures both gravity and an unknown translational acceleration (also known as external acceleration) in the body frame, while these algorithms assume that the accelerometer measures only the projection of gravity.This problem has received the attention of numerous studies in the literature.Four main approaches were adopted: (i) discard the accelerometer measurements momentarily [1], [2], [7], (ii) augment the state space by including a dynamic model for the external acceleration [8], (iii) estimate the external acceleration from additional sensors like Global Positionning System (GPS) [9], air-data systems or Doppler radar [10], (iv) filter out the external acceleration by assuming it is high frequency [11]; the first approach being the most popular.It involves computing the difference between the measured acceleration norm and the gravitational acceleration one.If the difference exceeds a predefined threshold, the algorithm considers the measured acceleration as unreliable.A significant performance loss is encountered for long-duration accelerated motion as the algorithm completely ignores the measured acceleration, relying solely on magnetometer measurements in the correction step.It is called the threshold-based adaptive method.The second approach is limited by the inaccuracy of the adopted dynamic models for the external acceleration while the third approach requires additional sensors.Finally, the highfrequency assumption in the last approach fails for scenarios involving constant or low-frequency external acceleration, leading to performance loss (e.g., ships and ground vehicles during their ramp-up and braking stages, as well as aircraft during takeoff and landing).
From this literature review, one can conclude that no solution satisfies the following three conditions simultaneously: 1) ability to handle long-duration accelerated motion, 2) no usage of extra sensors, 3) no prior information or assumptions on the external acceleration.For this purpose, in this letter, we consider the external acceleration as an unknown input that directly affects the output (measurement) function.Note that state estimation with unknown input has been the subject of several studies in the literature [12], [13], [14], but none of these works addressed the problem of considering the external acceleration as an unknown input.The approach in [15]  the first to address the problem of state estimation with unknown input for systems on SO(3), with the difference that the unknown input is affecting the state dynamic and not the output function.Exploiting a local approximation approach, the main contribution of this letter is the design of an estimation algorithm for a state on SO(3) jointly with an unknown input, with application to attitude and external acceleration estimation using a MARG sensor.This letter is structured as follows: Section II provides a concise mathematical background on the SO(3) group and the Gauss-Markov theorem.Section III explains the mathematical model expressed on SO(3) and the problem statement.Section IV introduces the filter equations and the algorithm steps.Section V describes and analyzes the proposed algorithm.Section VI presents the results from Monte Carlo simulations and real datasets, for the evaluation purpose.A conclusion in Section VII ends this letter.

II. BACKGROUND AND PRELIMINARIES
In this letter, matrices, vectors, and scalars are represented with bold upper-case symbols, bold lower-case symbols and lower-case symbols respectively.The symbols E(.), tr(.), x, I n and 0 stand for the Expectation operator, the trace operator, the estimate of a given variable x, the order n identity matrix, and the all zero matrix, respectively.

A. SO(3) Group and Skew-Symmetric Matrix
The special orthogonal group SO(3) refers to the group of all three-dimensional rotations that can be performed in space: SO(3) = {R ∈ R 3×3 : RR T = I 3 det(R) = 1} as explained in [4], [5].The exponential map exp m : R 3 → SO(3) maps between element ξ ∈ R 3 and element from SO(3) according to the formula [4]: exp m (0) = I 3 and exp m (ξ ) = , where (ξ ) × is the skew-symmetric matrix corresponding to the vector ξ ∈R 3 : The essential properties of the exponential map and the skew-symmetric matrix are recalled hereafter.For any vectors ξ , ξ 1 , ξ 2 ∈ R 3 , and small enough vectors v 1 , v 2 ∈ R 3 , we have these properties [15]: ( Given an estimate R of a rotation matrix R, the estimation error in SO( 3) is computed as R−1 R [4].It can be mapped to a vector ξ ∈ R 3 using the inverse of the exponential map.

B. Gauss-Markov Theorem [16]
Consider a model y = D x x + v, where v is a zero-mean random variable with positive definite covariance matrix R v , x is a deterministic vector, and D x has full column rank.Then the unbiased minimum-variance estimator of x is given by x

III. PROBLEM STATEMENT
Consider the following discrete-time state-space model [4], [15]: , where g ∈ R 3 and m e ∈ R 3 are the Earth's gravity acceleration and the Earth's magnetic field in the Earth's fixed frame.The external acceleration a ext k ∈ R 3 is unknown, and the matrix D is given by The system is left invertible (see [17,Definition 2.5]), meaning that the unknown input a ext k can be uniquely reconstructed given the outputs (y i ) i∈0,1,...,k , and the initial state R 0 .Moreover, the system is observable (see [17,Definition 2.4]), because the state R k can be uniquely reconstructed given the inputs (a ext i ) i∈0,1,...,k and the outputs (y i ) i∈0,1,...,k .Therefore, knowing the output sequence (y i ) i∈0,1,...,k and the initial state R 0 allows to uniquely reconstruct the unknown inputs (a ext i ) i∈0,1,...,k , and consequently the state R k .The objective of this letter is to design a filter that estimates R k in the presence of the unknown external acceleration a ext k , based on the sequences of the known input (ω i ) i∈0,1,...,k−1 and measurements (y i ) i∈0,1,...,k , and an initial estimate R0 , which is assumed to be unbiased with known covariance matrix P ξ 0 .This initial unbiased estimate is possible if a ext 0 is available.For this purpose, a novel recursive algorithm named UMV-SO(3)-EA is designed.It gives unbiased minimum variance first-order approximation-based estimator of R k based on the unbiased estimate of R k−1 , the known input ω k−1 , and the measurements y k .

IV. FILTER DESIGN
For linear discrete-time systems with unknown input having direct linear feedthrough to the output, the three-step Kalman filter was introduced in [14] for both state and unknown input estimation.The three steps are as follows: (i) predict the state using the state dynamic model, (ii) estimate the unknown input using the predicted state and the measurements, (iii) correct the predicted state using the estimated unknown input and the Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
, H k is the Jacobian of the output function with respect to the prediction error. 4: Correction: Based on the same principle, we propose a filter based on the following equations: where the matrices M k ∈ R 3×6 and K k ∈ R 3×6 are the gain matrices which have to be designed.The inputs of this filter are the estimated attitude Rk−1 with the corresponding covariance matrix P ξ k−1 , the angular velocity ω k−1 , and the measurements y k .The algorithm outputs are the estimated attitude Rk with the corresponding covariance matrix P ξ k , and the estimated external acceleration âext k with the corresponding covariance matrix P a k .Algorithm 1 presents the detailed steps of the proposed recursive filter UMV-SO(3)-EA.
In the next section, considering Rk−1 as unbiased, we show that the first-order approximation-based estimators of a ext k and R k given by Algorithm 1 are unbiased minimum variance (see Theorem 1 and Theorem 2).
Proof: Starting with the equation of prediction error exp m (ξ k|k−1 ) = R−1 k|k−1 R k , then employing the rotation dynamic (6) and the filter's first step (8) give: Applying first-order approximation (4) gives: and yields

B. External Acceleration Estimation
Lemma 2: Let Rk−1 be unbiased, then the difference ỹk = y k − h( Rk|k−1 ) found in the filter's second step ( 9) can be approximated by ỹk = Da ext k + e k , where e k is a random variable with zero mean and covariance matrix .
Proof: Substituting y k as written in (7) gives: The difference hk = h(R k ) − h( Rk|k−1 ) can be simplified as following: Replacing R k by Rk|k−1 exp m (ξ k|k−1 ) gives: then, using the first-order approximation (3), we obtain: Employing the skew-symmetric matrix property (5), gives: and substituting ( 13) in (12) gives: then: The expected value of e k is E(e k ) = 0 since E(ξ k|k−1 ) = 0 (See Lemma 1) and E(v y k ) = 0 (zero mean measurement noise).Its covariance matrix is: since the measurement noise v y k is independent of ξ k|k−1 .
Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
Theorem 1: Let Rk−1 be unbiased and the gain matrix , then the firstorder approximation-based estimator of a ext k provided by ( 9) is unbiased minimum variance.
Proof: Applying Lemma 2 enables us to use the approximated model (15) which satisfies Gauss Markov conditions (see Section II-B), where the matrix D has full column rank and Rk is positive definite, then the estimator ( 9) is unbiased and has minimum variance when the matrix gain M k is equal to: This gain matrix satisfies M k D = I 3 , we utilize this property after substituting (15) in (9): and finally: The corresponding covariance matrix is

C. Correction
Lemma 3: Let Rk−1 be unbiased and the matrix gain , then the first-order approximation-based estimator (10) is unbiased.
Theorem 2: Let Rk−1 be unbiased.Given the gains , then the first-order approximation-based estimator (10) is unbiased minimum variance.
Proof: Applying Lemma 3 proves that the estimator ( 10) is unbiased.Minimizing E( ξ k 2 ) is equivalent to minimizing the trace of the covariance matrix Then the aim is to prove that the proposed K k satisfies dK k = 0. We start by computing P ξ k , where we replace ξ k by (19): The term T can be simplified, by substituting (16), which gives 20) gives: and utilizing the matrix derivatives from [18, Propositions 10.7.2 and 10.7.4], leads to: dK k = 0 and the estimator is minimum variance.Finally by applying the value of K k in (21) gives

VI. EVALUATION OF UMV-SO(3)-EA: SIMULATIONS WITH THEORETICAL EXAMPLE AND REAL DATA
This section aims to validate the algorithm's effectiveness in different scenarios.We present results from both Monte Carlo simulations with 100 runs for each scenario, with different noise sequences for each run, and real datasets, ensuring the reliability of our findings.For the purpose of evaluation of the UMV-SO(3)-EA algorithm, we choose to compare it with the invariant extended Kalman filter (IEKF) [3], and also with the IEKF coupled with the threshold-based adaptive approach, to compensate the effects of the external acceleration on the estimation accuracy.We selected the threshold-based adaptive approach due to its popularity in the literature, and its ability to run without prior information, specific assumptions about the external acceleration, or a mathematical model of external acceleration.
In our practical scenarios, having an initial unbiased (but slightly perturbed) rotation estimate R0 is possible, where the external acceleration is known and equals to zero before the body starts moving, and the initial rotation can be obtained using other attitude estimation algorithms that depend only on accelerometer and magnetometer measurement like TRIAD [19].In both simulation and evaluation with real data, the three algorithms are initialized by a slightly perturbed rotation estimate, given by the TRIAD algorithm.In the following subsections, and when presenting the results, we compute the estimation error in a way that makes it more easily interpretable.This involves converting all rotation matrices to Euler angles using the XYZ convention, as detailed in [6,Sec. 3.3].
Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.

A. Threshold-Based Adaptive Approach
The concept behind the threshold-based adaptive approach involves computing the difference between the norm of the accelerometer output and the gravity norm of 9.81 m/s 2 , then comparing it to a predefined threshold value a (we used a = 0.2 m/s 2 [1]).If the difference exceeds the threshold during the time interval, it implies the presence of external acceleration.Consequently, higher values of the acceleration measurements' variances are employed in the IEKF considering the accelerometer measurements as unreliable.

B. Simulations Setup and Results
This subsection starts by explaining the simulation setup, and then the results will be presented in two parts: the first part includes figures that demonstrate the accuracy of the attitude and external acceleration estimation of the UMV-SO(3)-EA algorithm; the second part involves a comparison with the IEKF to which we add the threshold-based adaptive approach.Root Mean Square Error (RMSE) was calculated for each Monte Carlo run, and finally, the average was derived from the 100 runs.
1) Simulations Setup: For every Monte Carlo run, the system was run for a duration of 100 s, with a sampling time T = 0.01 s.The Earth's gravity and magnetic field were considered as g = [0, 0, 9.81] T m/s 2 and m e = [0.23,0.01, 0.41] T Gauss, respectively.The true values for body angular velocity and external acceleration were specified in accordance with Table I.The gyroscope, accelerometer, and magnetometer noises were set to be zero-mean white noise signals with standard deviations of σ ω = 0.01 rad/s 2 , σ a = 0.01 m/s 2 , and σ m = 0.005 Gauss, respectively.In order to have a fair comparison, the applied external acceleration shown in Table I is multiplied by a Bernoulli distributed white sequence taking values on {0, 1} with probabilities: A higher value of γ (i.e., closer to 1) means the body has more instances with the presence of external acceleration, and lower values (i.e., closer to 0) mean more instances with rest.

2) UMV-SO(3)-EA
Theoretical Estimation Results: We present the estimation results of the UMV-SO(3)-EA algorithm for a single Monte Carlo run, under the considered external acceleration for the full duration, i.e., γ = 1.Fig. 1 illustrates the estimation error.The errors for the three angles generally remain below 0.3 • most of the time.Although there are instances where the error exceeds 0.3 • , the algorithm demonstrates the ability to correct and achieve low-error estimation.Moreover, the UMV-SO(3)-EA algorithm effectively  estimates the external acceleration, and the estimation error, depicted in Fig. 2, is mostly ranging between −0.05 m/s 2 and 0.05 m/s 2 .The RMSE (calculated for the 100 Monte-Carlo runs) is 0.04 m/s 2 , aligning with the accelerometer's standard deviation range (4×σ a ).These findings indicate that the UMV-SO(3)-EA performs well and provides good results even when exposed to the considered external acceleration.

3) Comparison With the IEKF Coupled to the Threshold-
Based Adaptive Approach: Table II shows the RMSE of UMV-SO(3)-EA algorithm, the IEKF with and without adaptation for several scenarios depending on the value of γ , where γ = 1.0, γ = 0.7, γ = 0.55, and γ = 0.3 indicates 100%, 70%, 55%, and 30% of taking into account the external acceleration, respectively.In all scenarios, the IEKF with adaptation performs better than the IEKF without adaptation, because the latest relies on unreliable measurements.Across the same scenarios, UMV-SO(3)-EA demonstrates better performance to the IEKF with adaptation.This is because the IEKF with adaptation ignores acceleration measurements when detecting external acceleration.The accuracy of UMV-SO(3)-EA estimation is not affected significantly by a full or partial presence of external acceleration.In contrast, the accuracy of the IEKF with adaptation decreases when increasing the presence of external acceleration.This highlights that UMV-SO(3)-EA provides robust performance.

C. Evaluation With Real Data
In this subsection, we show the comparison results using the BROAD benchmark datasets [20].These datasets encompass various real movement scenarios, including rotation and translation, each stored in separate files identified by a trial ID.The datasets are thoroughly described in [20], providing details on sensors' noise variances and dataset characteristics.
For our comparison, we computed the RMSE over 50s after the initiation of movement.Note that there is an initial rest phase of approximately 30s, so each algorithm runs for around 80s for each scenario.We conducted this analysis for two different trials, numbered 16, and 23 with the results presented in Table III.
In trial 16, there is a significant external acceleration, with the measured acceleration norm reaching up to 10 times the Earth's gravity, persisting at a high value for most of the time.Trial 23, on the other hand, exhibits a lower acceleration norm, staying below 2 times the Earth's gravity, and having fewer instances with high measurement acceleration norms, mostly remaining close to the Earth's gravity.Our observations indicate that the UMV-SO(3)-EA consistently outperforms the IEKF with threshold-based adaptation.The improvement varies depending on the duration of external acceleration, and it is particularly significant during long periods of external acceleration.The accelerated motion duration of trial 16 is close to the simulated scenario with γ = 0.55.The IEKF with adaptation performs 2.8 times worse than UMV-SO(3)-EA in simulation, and around 1.5 times in real data.This difference could be due to several reasons, such as unmet assumptions regarding uncorrelated measurement noises, imprecise knowledge of covariance matrices, and sensor biases.

VII. CONCLUSION AND FUTURE WORK
This letter presented a novel attitude estimation algorithm in the presence of unknown external acceleration, using measurements from MARG sensors.The proposed UMV-SO(3)-EA was evaluated through Monte Carlo simulations and real datasets, and it was compared to the invariant extended Kalman filter (IEKF), with a threshold-based adaptation technique.The results, with both simulated and real data, showed that the UMV-SO(3)-EA performed precise and robust attitude estimation under significant external acceleration, and also outperformed the IEKF with threshold-based adaptation.The results showed also accurate external acceleration estimation of UMV-SO(3)-EA.In future work, the proposed algorithm will be extended to consider sensor biases.

∈
, for any time k, R k ∈ SO(3) is the attitude of the rigid body while y k = R 6 stands for the measurement vector containing accelerometer and magnetometer measurements a b k ∈ R 3 and b b k ∈ R 3 in the body frame, respectively, the function h being defined as h Tω meas k is obtained from the measured angular velocity ω meas k ∈ R 3 which is considered to be constant at time step k during the sampling time T. The process noise w k and the measurement noise v y k are assumed to be uncorrelated zero-mean white random signals with positive definite covariance matrices Q k and R k respectively.

TABLE III RMSE
(IN DEGREES) FOR UMV-SO(3)-EA, IEKF WITH ADAPTATION, AND IEKF WITHOUT ADAPTATION WHEN USING REAL DATASETS