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

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, Christophe Prieur

► To cite this version:

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, Christophe Prieur. MARG Sensor-Based Attitude Estimation On SO(3) Under Unknown External Acceleration. IEEE Control Systems Letters, 2023, 7, pp.3795-3800. 10.1109/LCSYS.2023.3342855. hal-04349499

HAL Id: hal-04349499

https://hal.science/hal-04349499v1

Submitted on 17 Dec 2023

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons CC BY 4.0 - Attribution - International License
# MARG Sensor-based Attitude Estimation on SO(3) Under Unknown External Acceleration

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, and Christophe Prieur

**Abstract**— 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 paper, 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.

**Index Terms**— Attitude estimation, External acceleration, Navigation, Unknown input, SO(3).

## I. INTRODUCTION

ATTITUDE estimation of rigid bodies plays an important 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]–[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 frames of the accelerometer and magnetometer, respectively, to satisfy the Wahba problem [6].

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 Positioning 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 high-frequency 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 paper, 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]–[14], but none of these works addressed the problem of considering the external acceleration as an unknown input. The approach in [15] is 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 paper 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.

The paper is structured as follows: Section II provides a

Ghadeer Shaaban*, Hassen Fourati*, Alain Kibangou*, and Christophe Prieur are with Univ. Grenoble Alpes, CNRS, Inria*, Grenoble INP, GIPSA-lab, Grenoble, France. Alain Kibangou is also with the Faculty of Science, University of Johannesburg (Auckland Park Campus), Johannesburg 2006, South Africa (e-mails: ghadeer.shaaban@gipsa-lab.fr; hassen.fourati@gipsa-lab.fr; alain.kibangou@gipsa-lab.fr; christophe.prieur@gipsa-lab.fr)
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 the paper.

## II. BACKGROUND AND PRELIMINARIES

In this paper, matrices, vectors, and scalars are represented with bold upper-case symbols, bold lower-case symbols and lower-case symbols respectively. The symbols $E(\cdot)$, $tr(\cdot)$, $\hat{x}$, $\mathbf{I}_n$ and $\mathbf{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) = {$\mathbf{R} \in \mathbb{R}^{3 \times 3} : \mathbf{R}\mathbf{R}^T = \mathbf{I}_3$, $\det(\mathbf{R}) = 1$} as explained in [4], [5]. The exponential map $\exp_m : \mathbb{R}^3 \to$ SO(3) maps between element $\xi \in \mathbb{R}^3$ and element from SO(3) according to the formula [4]: $\exp_m(\mathbf{0}) = \mathbf{I}_3$ and $\exp_m(\xi) = \mathbf{I}_3 + \frac{\sin(\|\xi\|)}{\|\xi\|} (\xi)_\times + 2 \frac{\sin(\|\xi\|/2)}{\|\xi\|} (\xi)_\times^2, \forall \xi \in \mathbb{R}^3 \setminus \{\mathbf{0}\}$, where $(\xi)_\times$ is the skew-symmetric matrix corresponding to the vector $\xi \in \mathbb{R}^3$:

$$ (\xi)_\times = \begin{pmatrix} \xi_1 \\ \xi_2 \\ \xi_3 \end{pmatrix}_\times = \begin{pmatrix} 0 & -\xi_3 & \xi_2 \\ \xi_3 & 0 & -\xi_1 \\ -\xi_2 & \xi_1 & 0 \end{pmatrix}. \quad (1) $$

The essential properties of the exponential map and the skew-symmetric matrix are recalled hereafter. For any vectors $\xi, \xi_1, \xi_2 \in \mathbb{R}^3$, and small enough vectors $\mathbf{v}_1, \mathbf{v}_2 \in \mathbb{R}^3$, we have these properties [15]:

$$ \exp_m(\xi)^{-1} = \exp_m(-\xi), \quad (2) $$

$$ \exp_m(\mathbf{v}_1) \approx \mathbf{I}_3 + (\mathbf{v}_1)_\times, \quad (3) $$

$$ \exp_m(\mathbf{v}_1 + \mathbf{v}_2) \approx \exp_m(\mathbf{v}_1)\exp_m(\mathbf{v}_2), \quad (4) $$

$$ (\xi_1)_\times \xi_2 = -(\xi_2)_\times \xi_1. \quad (5) $$

Given an estimate $\hat{\mathbf{R}}$ of a rotation matrix $\mathbf{R}$, the estimation error in SO(3) is computed as $\hat{\mathbf{R}}^{-1}\mathbf{R}$ [4]. It can be mapped to a vector $\xi \in \mathbb{R}^3$ using the inverse of the exponential map.

### B. Gauss-Markov Theorem [16]

Consider a model $\mathbf{y} = \mathbf{D}_x\mathbf{x} + \mathbf{v}$, where $\mathbf{v}$ is a zero-mean random variable with positive definite covariance matrix $\mathbf{R}_v$, $\mathbf{x}$ is a deterministic vector, and $\mathbf{D}_x$ has full column rank. Then the unbiased minimum-variance estimator of $\mathbf{x}$ is given by $\hat{\mathbf{x}} = (\mathbf{D}_x^T \mathbf{R}_v^{-1} \mathbf{D}_x)^{-1} \mathbf{D}_x^T \mathbf{R}_v^{-1} \mathbf{y}$.

## III. PROBLEM STATEMENT

Consider the following discrete-time state-space model [4], [15]:

$$ \mathbf{R}_{k+1} = \mathbf{R}_k \exp_m (\omega_k + \mathbf{w}_k), \quad (6) $$

$$ \mathbf{y}_k = h(\mathbf{R}_k) + \mathbf{D} \mathbf{a}_k^{\text{ext}} + \mathbf{v}_k^y, \quad (7) $$

where, for any time $k$, $\mathbf{R}_k \in \text{SO}(3)$ is the attitude of the rigid body while $\mathbf{y}_k = (\mathbf{a}_k^b \ \mathbf{b}_k^b) \in \mathbb{R}^6$ stands for the measurement vector containing accelerometer and magnetometer measurements $\mathbf{a}_k^b \in \mathbb{R}^3$ and $\mathbf{b}_k^b \in \mathbb{R}^3$ in the body frame, respectively, the function $h$ being defined as $h(\mathbf{R}_k) = (\mathbf{R}_k^{-1} \mathbf{g} \ \mathbf{R}_k^{-1} \mathbf{m}_e)$, where $\mathbf{g} \in \mathbb{R}^3$ and $\mathbf{m}_e \in \mathbb{R}^3$ are the Earth's gravity acceleration and the Earth's magnetic field in the Earth's fixed frame. The external acceleration $\mathbf{a}_k^{\text{ext}} \in \mathbb{R}^3$ is unknown, and the matrix $\mathbf{D}$ is given by $\mathbf{D} = (\mathbf{I}_3) \in \mathbb{R}^{6 \times 3}$. The input $\omega_k = \Delta T \omega_k^{\text{meas}}$ is obtained from the measured angular velocity $\omega_k^{\text{meas}} \in \mathbb{R}^3$ which is considered to be constant at time step $k$ during the sampling time $\Delta T$. The process noise $\mathbf{w}_k$ and the measurement noise $\mathbf{v}_k^y$ are assumed to be uncorrelated zero-mean white random signals with positive definite covariance matrices $\mathbf{Q}_k$ and $\mathbf{R}_k$ respectively.

The system is left invertible (see [17, Def. 2.5]), meaning that the unknown input $\mathbf{a}_k^{\text{ext}}$ can be uniquely reconstructed given the outputs $(\mathbf{y}_i)_{i \in 0,1,\dots,k}$, and the initial state $\mathbf{R}_0$. Moreover, the system is observable (see [17, Def. 2.4]), because the state $\mathbf{R}_k$ can be uniquely reconstructed given the inputs $(\mathbf{a}_i^{\text{ext}})_{i \in 0,1,\dots,k}$ and the outputs $(\mathbf{y}_i)_{i \in 0,1,\dots,k}$. Therefore, knowing the output sequence $(\mathbf{y}_i)_{i \in 0,1,\dots,k}$ and the initial state $\mathbf{R}_0$ allows to uniquely reconstruct the unknown inputs $(\mathbf{a}_i^{\text{ext}})_{i \in 0,1,\dots,k}$, and consequently the state $\mathbf{R}_k$.

The objective of this paper is to design a filter that estimates $\mathbf{R}_k$ in the presence of the unknown external acceleration $\mathbf{a}_k^{\text{ext}}$, based on the sequences of the known input $(\omega_i)_{i \in 0,1,\dots,k-1}$ and measurements $(\mathbf{y}_i)_{i \in 0,1,\dots,k}$, and an initial estimate $\mathbf{R}_0$, which is assumed to be unbiased with known covariance matrix $\mathbf{P}_0^\xi$. This initial unbiased estimate is possible if $\mathbf{a}_0^{\text{ext}}$ 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 $\mathbf{R}_k$ based on the unbiased estimate of $\mathbf{R}_{k-1}$, the known input $\omega_{k-1}$, and the measurements $\mathbf{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 measurements. Based on the same principle, we propose a
filter based on the following equations:

$$ \hat{\mathbf{R}}_{k|k-1} = \hat{\mathbf{R}}_{k-1} \exp_m(\omega_{k-1}), \quad (8) $$

$$ \hat{\mathbf{a}}_{k}^{\text{ext}} = \mathbf{M}_{k} \left( \mathbf{y}_{k} - h(\hat{\mathbf{R}}_{k|k-1}) \right), \quad (9) $$

$$ \hat{\mathbf{R}}_{k} = \hat{\mathbf{R}}_{k|k-1} \exp_m \left( \mathbf{K}_{k} \left( \mathbf{y}_{k} - h(\hat{\mathbf{R}}_{k|k-1}) - \mathbf{D}\hat{\mathbf{a}}_{k}^{\text{ext}} \right) \right), \quad (10) $$

where the matrices $\mathbf{M}_k \in \mathbb{R}^{3 \times 6}$ and $\mathbf{K}_k \in \mathbb{R}^{3 \times 6}$ are the gain matrices which have to be designed. The inputs of this filter are the estimated attitude $\hat{\mathbf{R}}_{k-1}$ with the corresponding covariance matrix $\mathbf{P}_{k-1}^{\xi}$, the angular velocity $\omega_{k-1}$, and the measurements $\mathbf{y}_k$. The algorithm outputs are the estimated attitude $\hat{\mathbf{R}}_k$ with the corresponding covariance matrix $\mathbf{P}_k^{\xi}$, and the estimated external acceleration $\hat{\mathbf{a}}_{k}^{\text{ext}}$ with the corresponding covariance matrix $\mathbf{P}_k^a$. Algorithm 1 presents the detailed steps of the proposed recursive filter UMV-SO(3)-EA.

### Algorithm 1 UMV-SO(3)-EA

$$ \text{Require: } \hat{\mathbf{R}}_{k-1}, \mathbf{P}_{k-1}^{\xi}, \omega_{k-1}, \mathbf{y}_k = \begin{pmatrix} \mathbf{a}_k^b \\ \mathbf{b}_k^b \end{pmatrix} $$

▷ Prediction:

$$ \text{1: } \hat{\mathbf{R}}_{k|k-1} = \hat{\mathbf{R}}_{k-1} \exp_m(\omega_{k-1}) $$

$$ \text{2: } \mathbf{P}_{k|k-1}^{\xi} = \mathbf{P}_{k-1}^{\xi} + \mathcal{Q}_{k-1} $$

▷ External acceleration estimation:

$$ \text{3: } \mathbf{H}_k = \begin{pmatrix} (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g}) \times \\ (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e) \end{pmatrix}, \quad \mathbf{H}_k \text{ is the Jacobian of the output function with respect to the prediction error.} $$

$$ \text{4: } \tilde{\mathbf{R}}_k = \mathbf{H}_k \mathbf{P}_{k|k-1}^{\xi} \mathbf{H}_k^T + \mathbf{R}_k $$

$$ \text{5: } \mathbf{M}_k = (\mathbf{D}^T \tilde{\mathbf{R}}_k^{-1} \mathbf{D})^{-1} \mathbf{D}^T \tilde{\mathbf{R}}_k^{-1} $$

$$ \text{6: } \hat{\mathbf{a}}_{k}^{\text{ext}} = \mathbf{M}_{k} \left( \mathbf{y}_{k} - h(\hat{\mathbf{R}}_{k|k-1}) \right) $$

$$ \text{7: } \mathbf{P}_{k}^{a} = (\mathbf{D}^{T} \tilde{\mathbf{R}}_{k}^{-1} \mathbf{D})^{-1} $$

▷ Correction:

$$ \text{8: } \mathbf{K}_k = \mathbf{P}_{k|k-1}^{\xi} \mathbf{H}_k^T \tilde{\mathbf{R}}_k^{-1} $$

$$ \text{9: } \hat{\mathbf{R}}_k = \hat{\mathbf{R}}_{k|k-1} \exp_m \left( \mathbf{K}_k \left( \mathbf{y}_k - h(\hat{\mathbf{R}}_{k|k-1}) - \mathbf{D}\hat{\mathbf{a}}_{k}^{\text{ext}} \right) \right) $$

$$ \text{10: } \mathbf{P}_{k|k-1}^{\xi} = \mathbf{P}_{k|k-1}^{\xi} - \mathbf{K}_k (\mathbf{I}_6 - \mathbf{D}\mathbf{M}_k) \mathbf{H}_k \mathbf{P}_{k|k-1}^{\xi} $$

$$ \text{11: } \textbf{return } \hat{\mathbf{R}}_k, \mathbf{P}_k^\xi, \hat{\mathbf{a}}_k^{\text{ext}}, \mathbf{P}_k^a $$

In the next section, considering $\hat{\mathbf{R}}_{k-1}$ as unbiased, we show that the first-order approximation-based estimators of $\hat{\mathbf{a}}_{k}^{\text{ext}}$ and $\mathbf{R}_k$ given by Algorithm 1 are unbiased minimum variance (see Theorem 5.3 and Theorem 5.5).

## V. FILTER ANALYSIS

We define the estimation error $\exp_m(\xi_k) = \hat{\mathbf{R}}_k^{-1} \mathbf{R}_k$, and the prediction error $\exp_m(\xi_{k|k-1}) = \mathbf{R}_{k|k-1}^{-1} \mathbf{R}_k$, with the corresponding covariance matrices $\mathbf{P}_k^\xi = E(\xi_k \xi_k^T)$, and $\mathbf{P}_{k|k-1}^\xi = E(\xi_{k|k-1} \xi_{k|k-1}^T)$ respectively.

### A. Prediction

**Lemma 5.1:** Let $\hat{\mathbf{R}}_{k-1}$ be unbiased, then the first-order approximation-based estimator (8) is unbiased.

*Proof:* Starting with the equation of prediction error $\exp_m(\xi_{k|k-1}) = \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{R}_k$, then employing the rotation dynamic (6) and the filter's first step (8) give: $\exp_m(\xi_{k|k-1}) = \exp_m(-\omega_{k-1}) \hat{\mathbf{R}}_{k-1}^{-1} \mathbf{R}_{k-1} \exp_m(\omega_{k-1} + \mathbf{w}_{k-1}) = \exp_m(-\omega_{k-1}) \exp_m(\xi_{k-1}) \exp_m(\omega_{k-1} + \mathbf{w}_{k-1})$. Applying first-order approximation (4) gives:

$$ \xi_{k|k-1} = \xi_{k-1} + \mathbf{w}_{k-1}. \quad (11) $$

and yields $E(\xi_{k|k-1}) = E(\xi_{k-1}) + E(\mathbf{w}_{k-1}) = \mathbf{0}$ since $E(\xi_{k-1}) = E(\mathbf{w}_{k-1}) = \mathbf{0}$. Its covariance matrix is $\mathbf{P}_{k|k-1}^\xi = E(\xi_{k|k-1} \xi_{k|k-1}^T) = \mathbf{P}_{k-1}^\xi + \mathcal{Q}_{k-1}$. ■

### B. External acceleration estimation

**Lemma 5.2:** Let $\hat{\mathbf{R}}_{k-1}$ be unbiased, then the difference $\tilde{\mathbf{y}}_k = \mathbf{y}_k - h(\hat{\mathbf{R}}_{k|k-1})$ found in the filter's second step (9) can be approximated by $\tilde{\mathbf{y}}_k = \mathbf{D}\mathbf{a}_k^{\text{ext}} + \mathbf{e}_k$, where $\mathbf{e}_k$ is a random variable with zero mean and covariance matrix $\tilde{\mathbf{R}}_k = E(\mathbf{e}_k \mathbf{e}_k^T) = \mathbf{H}_k \mathbf{P}_{k|k-1}^\xi \mathbf{H}_k^T + \mathbf{R}_k$, and $\mathbf{H}_k = \begin{pmatrix} (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g}) \times \\ (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e) \times \end{pmatrix}$. ■

*Proof:* Substituting $\mathbf{y}_k$ as written in (7) gives:

$$ \tilde{\mathbf{y}}_k = h(\mathbf{R}_k) - h(\hat{\mathbf{R}}_{k|k-1}) + \mathbf{D}\mathbf{a}_k^{\text{ext}} + \mathbf{v}_k^y. \quad (12) $$

The difference $\tilde{\mathbf{h}}_k = h(\mathbf{R}_k) - h(\hat{\mathbf{R}}_{k|k-1})$ can be simplified as following:

$$ \tilde{\mathbf{h}}_k = \begin{pmatrix} \mathbf{R}_k^{-1} \mathbf{g} \\ \mathbf{R}_k^{-1} \mathbf{m}_e \end{pmatrix} - \begin{pmatrix} \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g} \\ \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e \end{pmatrix}, \\ = \begin{pmatrix} \mathbf{R}_k^{-1} - \hat{\mathbf{R}}_{k|k-1}^{-1} \\ \mathbf{R}_k^{-1} - \hat{\mathbf{R}}_{k|k-1}^{-1} \end{pmatrix} \mathbf{g} \\ \mathbf{m}_e \end{pmatrix} $$

Replacing $\mathbf{R}_k$ by $\hat{\mathbf{R}}_{k|k-1} \exp_m(\xi_{k|k-1})$ gives:

$$ \tilde{\mathbf{h}}_k = \begin{pmatrix} (\exp_m(-\xi_{k|k-1}) - \mathbf{I}_3) \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g} \\ (\exp_m(-\xi_{k|k-1}) - \mathbf{I}_3) \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e \end{pmatrix}, $$

then, using the first-order approximation (3), we obtain:

$$ \tilde{\mathbf{h}}_k = \begin{pmatrix} -(\xi_{k|k-1}) \times \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g} \\ -(\xi_{k|k-1}) \times \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e \end{pmatrix}. $$

Employing the skew-symmetric matrix property (5), gives:

$$ \tilde{\mathbf{h}}_k = \begin{pmatrix} (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g}) \times \xi_{k|k-1} \\ (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e) \times \xi_{k|k-1} \end{pmatrix} = \mathbf{H}_k \xi_{k|k-1}, \quad (13) $$

and substituting (13) in (12) gives: $\tilde{\mathbf{y}}_k = \mathbf{D}\mathbf{a}_k^{\text{ext}} + \mathbf{H}_k \xi_{k|k-1} + \mathbf{v}_k^y$.

We define

$$ \mathbf{e}_k = \mathbf{H}_k \xi_{k|k-1} + \mathbf{v}_k^y, \quad (14) $$

then:

$$ \tilde{\mathbf{y}}_k = \mathbf{D}\mathbf{a}_k^{\text{ext}} + \mathbf{e}_k. \quad (15) $$

The expected value of $\mathbf{e}_k$ is $E(\mathbf{e}_k) = \mathbf{0}$ since $E(\xi_{k|k-1}) = \mathbf{0}$ (See Lemma 5.1) and $E(\mathbf{v}_k^y) = \mathbf{0}$ (zero mean measurement noise). Its covariance matrix is:

$$ \tilde{\mathbf{R}}_k = E(\mathbf{e}_k \mathbf{e}_k^T) = \mathbf{H}_k \mathbf{P}_{k|k-1}^\xi \mathbf{H}_k^T + \mathbf{R}_k, \quad (16) $$

since the measuerent noise $\mathbf{v}_k^y$ is independent of $\xi_{k|k-1}$. ■
## 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 \text{ m/s}^2$, then comparing it to a predefined threshold value $\epsilon_a$ (we used $\epsilon_a = 0.2 \text{ 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 $\Delta T = 0.01 \text{ s}$. The Earth's gravity and magnetic field were considered as $g = [0, 0, 9.81]^T \text{ m/s}^2$ and $m_e = [0.23, 0.01, 0.41]^T \text{ 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 $\sigma_\omega = 0.01 \text{ rad/s}^2$, $\sigma_a = 0.01 \text{ m/s}^2$, and $\sigma_m = 0.005 \text{ Gauss}$, respectively. In

<table><caption>TABLE I
THE TRUE ANGULAR VELOCITY AND EXTERNAL ACCELERATION USED IN SIMULATION</caption><thead><tr><th colspan="2">True angular velocity<br>(rad/s)</th><th colspan="2">True external acceleration<br>(m/s<sup>2</sup>)</th></tr></thead><tbody><tr><td>ω<sub>x</sub></td><td>2.0 cos (0.2πkΔT)</td><td>a<sup>ext</sup><sub>e</sub></td><td>2.0 sin (0.5πkΔT)γ<sub>k</sub></td></tr><tr><td>ω<sub>y</sub></td><td>1.5 cos (0.6πkΔT)</td><td>a<sup>ext</sup><sub>y</sub></td><td>1.0 sin (0.2πkΔT)γ<sub>k</sub></td></tr><tr><td>ω<sub>z</sub></td><td>1.0 cos (1.0πkΔT)</td><td>a<sup>ext</sup><sub>z</sub></td><td>0.5 sin (0.1πkΔT)γ<sub>k</sub></td></tr></tbody></table>

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:

$$Pr\{\gamma_k = 1\} = \bar{\gamma}, \quad Pr\{\gamma_k = 0\} = 1 - \bar{\gamma}$$

A higher value of $\bar{\gamma}$ (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. $\bar{\gamma} = 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 \text{ m/s}^2$ and $0.05 \text{ m/s}^2$. The RMSE (calculated for the 100 Monte-Carlo runs) is $0.04 \text{ m/s}^2$, aligning with the accelerometer's standard deviation range ($4 \times \sigma_a$). These findings indicate that the UMV-SO(3)-EA performs well and provides good results even when exposed to the considered external acceleration.

<figure><img src="image_2.png" /><figcaption>Fig. 1. Attitude estimation error of UMV-SO(3)-EA represented as Euler angles</figcaption></figure>

<figure><img src="image_3.png" /><figcaption>Fig. 2. External acceleration estimation error $a^{ext} - \hat{a}^{ext}$ of UMV-SO(3)-EA</figcaption></figure>

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 $\bar{\gamma}$, where $\bar{\gamma} = 1.0$, $\bar{\gamma} = 0.7$, $\bar{\gamma} = 0.55$, and $\bar{\gamma} = 0.3$ indicates 100%, 70%, 55%, and 30% of taking into account the external acceleration, respectively. In all scenarios, the IEKF with adaptation performs

<table><caption>TABLE II
RMSE (IN DEGREES) FOR UMV-SO(3)-EA, IEKF WITH ADAPTATION, AND IEKF WITHOUT ADAPTATION IN SIMULATION</caption><thead><tr><th rowspan="2">γ̃</th><th>UMV-SO(3)-EA</th><th>IEKF<br>with adaptation</th><th>IEKF<br>without adaptation</th></tr><tr><th>RMSE (degrees)</th><th>RMSE (degrees)</th><th>RMSE (degrees)</th></tr></thead><tbody><tr><td>1.0</td><td>0.43</td><td>7.64</td><td>9.90</td></tr><tr><td>0.7</td><td>0.42</td><td>1.92</td><td>6.17</td></tr><tr><td>0.55</td><td>0.42</td><td>1.19</td><td>4.53</td></tr><tr><td>0.3</td><td>0.42</td><td>0.72</td><td>2.24</td></tr></tbody></table>

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

<table><caption>TABLE III
RMSE (IN DEGREES) FOR UMV-SO(3)-EA, IEKF WITH ADAPTATION, AND IEKF WITHOUT ADAPTATION WHEN USING REAL DATASETS</caption><thead><tr><th>Trial ID<br>In [20]</th><th>UMV-SO(3)-EA<br>RMSE (degrees)</th><th>IEKF<br>with adaptation<br>RMSE (degrees)</th><th>IEKF<br>without adaptation<br>RMSE (degrees)</th></tr></thead><tbody><tr><td>16</td><td>7.23</td><td>10.96</td><td>40.56</td></tr><tr><td>23</td><td>4.42</td><td>5.50</td><td>6.65</td></tr></tbody></table>

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 $\gamma = 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 paper 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.

## REFERENCES

[1] A. M. Sabatini, "Rotator-based extended Kalman filter for determining orientation by inertial and magnetic sensing," *IEEE Trans. Biomed. Eng.*, vol. 53, no. 7, pp. 1346-1356, 2006.

[2] Y. S. Suh, "Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration," *IEEE Trans. Instrum. and Meas.*, vol. 59, no. 12, pp. 3296-3305, 2010.

[3] A. Barrau and S. Bonnabel, "Invariant Kalman filtering," *Annu. Rev. Control, Robot., Auton. Syst.*, vol. 1, no. 1, pp. 237-257, 2018.

[4] S. Bonnabel and A. Barrau, "The geometry of navigation problems," *IEEE Trans. Automat. Control*, vol. 68, no. 2, pp. 689-704, 2023.

[5] J. Reis, P. Batista, P. Oliveira, and C. Silvestre, "Nonlinear observer on SO(3) for attitude estimation on rotating earth using single vector measurements," *IEEE Control Syst. Lett.*, vol. 3, no. 2, pp. 392-397, 2019.

[6] H. Fourati and D. E. C. Belkhiat, Eds., *Multisensor attitude estimation: fundamental concepts and applications*. CRC Press, 2016.

[7] H. Rehbinder and X. Hu, "Drift-free attitude estimation for accelerated rigid bodies," *Automatica*, vol. 40, no. 4, pp. 653-659, 2004.

[8] J. K. Lee, E. J. Park, and S. N. Robinovitch, "Estimation of attitude and external acceleration using inertial sensor measurement during various dynamic conditions," *IEEE Trans. Instrum. and Meas.*, vol. 61, no. 8, pp. 2262-2273, 2012.

[9] T. A. Johansen, J. M. Hansen, and T. I. Fossen, "Nonlinear observer for tightly integrated inertial navigation aided by pseudo-range measurements," *J. Dyn. Sys., Meas., Control*, vol. 139, no. 1, p. 011007, 2017.

[10] M.-D. Hua, P. Martin, and T. Hamel, "Stability analysis of velocity-aided attitude observers for accelerated vehicles," *Automatica*, vol. 63, pp. 11-15, 2016.

[11] T. Bonargent, T. Menard, E. Pigeon, and O. Gehan, "Observer and first-order low-pass filter based attitude estimation for rigid bodies subject to external acceleration," in *IEEE 58th Conf. Decis. Control (CDC)*, Nice, France, 2019, pp. 629-634.

[12] S. Z. Yong, M. Zhu, and E. Frazzoli, "A unified filter for simultaneous input and state estimation of linear discrete-time stochastic systems," *Automatica*, vol. 63, pp. 321-329, 2016.

[13] S. Gillijns and B. De Moor, "Unbiased minimum-variance input and state estimation for linear discrete-time systems," *Automatica*, vol. 43, no. 1, pp. 111-116, 2007.

[14] —, "Unbiased minimum-variance input and state estimation for linear discrete-time systems with direct feedthrough," *Automatica*, vol. 43, no. 5, pp. 934-937, 2007.

[15] G. Shaaban, H. Fourati, A. Kibangou, and C. Prieur, "Gyro-free Kalman filter with unknown inputs for SO(3)-based attitude estimation," in *IEEE 13th Int. Conf. Indoor Position. Indoor Navig. (IPIN)*, Nuremberg, Germany, 2023.

[16] T. Kailath, A. H. Sayed, and B. Hassibi, *Linear estimation*. Prentice Hall, 2000.

[17] S. Sundaram, "Fault-tolerant and secure control systems," 2012, Univ. Waterloo, Waterloo, ON, Canada, Lecture Notes, 2012.

[18] D. S. Bernstein, *Matrix mathematics: theory, facts, and formulas*, 2nd ed. Princeton University Press, 2009.

[19] M. D. Shuster and S. D. Oh, "Three-axis attitude determination from vector observations," *J. Guid. Control Dyn.*, vol. 4, no. 1, pp. 70-77, 1981.

[20] D. Laidig, M. Caruso, A. Cereatti, and T. Seel, "BROAD—A benchmark for robust inertial orientation estimation," *Data*, vol. 6, no. 7, p. 72, 2021.