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

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, Christophe Prieur

► To cite this version:

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, Christophe Prieur. Gyro-Free Kalman Filter with Unknown Inputs for SO(3)-based Attitude Estimation. IPIN 2023 - 13th International Conference on Indoor Positioning and Indoor Navigation, Sep 2023, Nuremberg, Germany. pp.1-6, 10.1109/IPIN57070.2023.10332509. hal-04213739

HAL Id: hal-04213739

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

Submitted on 21 Sep 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
# Gyro-Free Kalman Filter with Unknown Inputs for SO(3)-based Attitude Estimation

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

Univ. Grenoble Alpes, CNRS, Inria*, Grenoble INP, GIPSA-lab, Grenoble, France

e-mails: firstname.lastname@gipsa-lab.grenoble-inp.fr

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

***Index Terms***—Gyro-free, attitude estimation, navigation, unknown input, SO(3).

## 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 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 low-cost 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 3-axis 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 $\mathbf{A}$ that meet the criterion: $\mathbf{A}^T = -\mathbf{A}$. Three-by-three skew-symmetric matrix $(\xi)_\times$ that is corresponding to a vector $\xi \in \mathbb{R}^3$ is written as:

$$ (\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) $$

We can perform cross-product operations between two vectors in $\mathbb{R}^3$ in the form of matrix multiplications. Consider $\mathbf{v}_1, \mathbf{v}_2 \in \mathbb{R}^3$, then $\mathbf{v}_1 \times \mathbf{v}_2 = (\mathbf{v}_1)_\times \mathbf{v}_2$. As $\mathbf{v}_1 \times \mathbf{v}_2 = -\mathbf{v}_2 \times \mathbf{v}_1$, we conclude with this useful property:

$$ (\mathbf{v}_1)_\times \mathbf{v}_2 = -(\mathbf{v}_2)_\times \mathbf{v}_1. \quad (2) $$

**Lemma 1.** Let $\mathbf{u}$ and $\mathbf{w}$ be two linearly independent vectors in $\mathbb{R}^3$. Then, the matrix $\begin{pmatrix} (\mathbf{u})_\times \\ (\mathbf{w})_\times \end{pmatrix}$ has full column rank.

*Proof.* Let us assume that the matrix $\begin{pmatrix} (\mathbf{u})_\times \\ (\mathbf{w})_\times \end{pmatrix}$ is not a full column rank. This means that there exists a vector $\mathbf{f} \in \mathbb{R}^3 \setminus \{0\}$ such that $\begin{pmatrix} (\mathbf{u})_\times \\ (\mathbf{w})_\times \end{pmatrix} \mathbf{f} = \mathbf{0}$ then $(\mathbf{u})_\times \mathbf{f} = \mathbf{0}$ and $(\mathbf{w})_\times \mathbf{f} = \mathbf{0}$, which give us $\mathbf{u} \times \mathbf{f} = \mathbf{0}$ and $\mathbf{w} \times \mathbf{f} = \mathbf{0}$, and thus both $\mathbf{u}$ and $\mathbf{f}$ are linearly dependent, and similarly, both $\mathbf{w}$ and $\mathbf{f}$ are linearly dependent. Consequently, $\mathbf{w}$ and $\mathbf{u}$ are linearly dependent. Therefore, the initial assumption that the matrix $\begin{pmatrix} (\mathbf{u})_\times \\ (\mathbf{w})_\times \end{pmatrix}$ does not have a full-column rank must be false. Hence, it has indeed a full-column rank. $\square$

### 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(3) = {$\mathbf{R} \in \mathbb{R}^{3 \times 3} : \mathbf{R}\mathbf{R}^T = \mathbf{I}_3, \det(\mathbf{R}) = 1$} [17]. The exponential map $\exp_m : \mathbb{R}^3 \to \text{SO}(3)$ allows transferring element $\xi \in \mathbb{R}^3$ to the SO(3) accordingly to the following formula [17]:

$$ \exp_m(\xi) = \mathbf{I}_3 + \frac{\sin(\|\xi\|)}{\|\xi\|}(\xi)_\times + 2 \frac{\sin(\|\xi\|/2)^2}{\|\xi\|^2}(\xi)_\times^2. \quad (3) $$

Some key properties of the exponential map [4]:

$$ \exp_m(\xi) = \exp((\xi)_\times), \quad (4) $$

$$ \exp_m(\xi)^{-1} = \exp_m(-\xi). \quad (5) $$

First-order approximation of (3) when $\|\xi\| \ll 1$ gives:

$$ \exp_m(\xi) \approx \mathbf{I}_3 + (\xi)_\times. \quad (6) $$

For small $\mathbf{v}_1, \mathbf{v}_2 \in \mathbb{R}^3$ (i.e $\|\mathbf{v}_1\| \ll 1, \|\mathbf{v}_2\| \ll 1$), we also have this approximation [5]:

$$ \exp_m(\mathbf{v}_1 + \mathbf{v}_2) \approx \exp_m(\mathbf{v}_1)\exp_m(\mathbf{v}_2). \quad (7) $$

### C. Attitude dynamics represented on SO(3)

The discrete-time rotation dynamics of a rigid body can be expressed as [18], [19]:

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

where the subscript $k$ denotes the time step number, $\mathbf{R}_k \in \mathbb{R}^{3 \times 3}$ is the rotation matrix from the body frame to the Earth-fixed frame, $\omega_k = \Delta T \omega_k^{\text{true}}$ where $\omega_k^{\text{true}} \in \mathbb{R}^3$ is the true angular velocity and $\Delta T$ is the sampling time, $\mathbf{w}_k$ is the process noise which is assumed to follow a Gaussian distribution with a mean of zero and a covariance matrix $\mathbf{Q}_k$. The measurement equation, expressed as $\mathbf{Y}_k \in \mathbb{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 $\mathbf{Y}_k$ is given by:

$$ \mathbf{Y}_k = \begin{pmatrix} \mathbf{a}_k^b \\ \mathbf{B}_k^b \end{pmatrix} = \begin{pmatrix} \mathbf{R}_k^{-1} \mathbf{g} \\ \mathbf{R}_k^{-1} \mathbf{m}_e \end{pmatrix} + \begin{pmatrix} \mathbf{v}_k^g \\ \mathbf{v}_k^m \end{pmatrix}, \quad (9) $$

where $\mathbf{a}_k^b \in \mathbb{R}^3$, $\mathbf{B}_k^b \in \mathbb{R}^3$ are the accelerometer and magnetometer measurements written in the body frame, respectively. $\mathbf{g} \in \mathbb{R}^3$, $\mathbf{m}_e \in \mathbb{R}^3$ are the Earth's gravity acceleration and the Earth's magnetic field written in the Earth's fixed frame, respectively. $\mathbf{v}_k^y = \begin{pmatrix} \mathbf{v}_k^g \\ \mathbf{v}_k^m \end{pmatrix}$ represents the measurement noise vector, which is assumed to follow a Gaussian distribution with a mean of zero and a covariance matrix $\mathcal{R}_k$. Also, the measurement function is defined by $h: \mathbf{R} \mapsto \begin{pmatrix} \mathbf{R}^{-1}\mathbf{g} \\ \mathbf{R}^{-1}\mathbf{m}_e \end{pmatrix}$, then $\mathbf{Y}_k$ could be re-written as:

$$ \mathbf{Y}_k = h(\mathbf{R}_k) + \mathbf{v}_k^y. \quad (10) $$

### D. Problem statement

Equations (8) and (10) define the discrete-time system model:

$$ \begin{cases} \mathbf{R}_{k+1} = \mathbf{R}_k \exp_m(\omega_k + \mathbf{w}_k), \\ \mathbf{Y}_k = h(\mathbf{R}_k) + \mathbf{v}_k^y. \end{cases} $$

Our aim is to design an estimator for $(\mathbf{R}_k)_{k \in \mathbb{N}^*}$ when the input $(\omega_k)_{k \in \mathbb{N}}$ is unknown using only the measurements $(\mathbf{Y}_k)_{k \in \mathbb{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:

$$ \mathbf{x}_{k+1} = \mathbf{A}_k \mathbf{x}_k + \mathbf{E}_k \mathbf{d}_k + \mathbf{w}_k, \quad (11) $$

$$ \mathbf{y}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k, \quad (12) $$

where $\mathbf{x}_k \in \mathbb{R}^n$, $\mathbf{d}_k \in \mathbb{R}^p$, $\mathbf{y}_k \in \mathbb{R}^m$ are the system state, the unknown input, and the system output (measurements), respectively. The matrix $\mathbf{E}_k$ has the appropriate rank: $\text{rank}[\mathbf{E}_k] = p$. $\mathbf{w}_k$, $\mathbf{v}_k$ are the process and measurement noise assumed to follow Gaussian distributions with mean of zero and covariance matrix of $\mathcal{Q}_k$ and $\mathbf{R}_k$ respectively. Certain conditions related to the matrices $\mathbf{A}_k$, $\mathbf{E}_k$, and $\mathbf{H}_k$ need to be satisfied to make the RTSKF works properly. These conditions are as follows [12]:

$$ \begin{cases} \text{rank}[\mathbf{H}_k] = m \ge p, \\ \text{rank}[\mathbf{H}_k \mathbf{E}_{k-1}] = p. \end{cases} \quad (13) $$

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 $(\hat{\mathbf{x}}_{k-1}$ and $\mathbf{P}_{k-1}^x$ respectively) and the output vector in current time step $(\mathbf{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 $\mathbf{K}_k^x$ to correct the predicted state in step 6 in order to minimize the state estimation error covariance matrix $\hat{\mathbf{P}}_k^x$ in step 7. Steps 8, 9 and 10 estimate the unknown input $\hat{\mathbf{d}}_k$ with a minimum covariance matrix $\mathbf{P}_k^d$. Finally, steps 11, 12 and 13 give the final state estimation $\hat{\mathbf{x}}_k$ with the final state estimation error covariance matrix $\mathbf{P}_k^x$ 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

Input: $\hat{\mathbf{x}}_{k-1}$, $\mathbf{P}_{k-1}^x$, $\mathbf{y}_k$

$$ \text{1: } \hat{\mathbf{x}}_{k|k-1} = \mathbf{A}_{k-1} \hat{\mathbf{x}}_{k-1} $$

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

$$ \text{3: } \hat{\mathbf{y}}_k = \mathbf{y}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} $$

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

$$ \text{5: } \mathbf{K}_k^x = \mathbf{P}_{k|k-1}^x \mathbf{H}_k^T \mathbf{C}_k^{-1} $$

$$ \text{6: } \bar{\mathbf{x}}_k = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k^x \tilde{\mathbf{y}}_k $$

$$ \text{7: } \mathbf{P}_k^x = (\mathbf{I} - \mathbf{K}_k^x \mathbf{H}_k) \mathbf{P}_{k|k-1}^x $$

$$ \text{8: } \mathbf{P}_k^d = (\mathbf{E}_{k-1}^T \mathbf{H}_k^T \mathbf{C}_k^{-1} \mathbf{H}_k \mathbf{E}_{k-1})^{-1} $$

$$ \text{9: } \mathbf{K}_k^d = \mathbf{P}_k^d \mathbf{E}_{k-1}^T \mathbf{H}_k^T \mathbf{C}_k^{-1} $$

$$ \text{10: } \hat{\mathbf{d}}_k = \mathbf{K}_k^d \tilde{\mathbf{y}}_k $$

$$ \text{11: } \mathbf{V}_k = (\mathbf{I} - \mathbf{K}_k^x \mathbf{H}_k) \mathbf{E}_{k-1} $$

$$ \text{12: } \hat{\mathbf{x}}_k = \bar{\mathbf{x}}_k + \mathbf{V}_k \hat{\mathbf{d}}_k $$

$$ \text{13: } \mathbf{P}_k^x = \hat{\mathbf{P}}_k^x + \mathbf{V}_k \mathbf{P}_k^d \mathbf{V}_k^T $$

$$ \text{14: } \textbf{return } \hat{\mathbf{x}}_k, \mathbf{P}_k^x $$

Let us define the time update estimation error $\tilde{\mathbf{x}}_{k|k-1} = \mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1}$ and the estimation error $\tilde{\mathbf{x}}_k = \mathbf{x}_k - \hat{\mathbf{x}}_k$. By applying (11) and step 1 of Algorithm 1 in the time update estimation error equation, we get:

$$ \begin{aligned} \tilde{\mathbf{x}}_{k|k-1} &= \mathbf{A}_{k-1} \mathbf{x}_{k-1} + \mathbf{E}_{k-1} \mathbf{d}_{k-1} + \mathbf{w}_{k-1} - \mathbf{A}_{k-1} \hat{\mathbf{x}}_{k-1}, \\ \tilde{\mathbf{x}}_{k|k-1} &= \mathbf{A}_{k-1} \tilde{\mathbf{x}}_{k-1} + \mathbf{E}_{k-1} \mathbf{d}_{k-1} + \mathbf{w}_{k-1}. \end{aligned} \quad (14) $$

The innovation can be written as follows:

$$ \tilde{\mathbf{y}}_k = \mathbf{y}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} $$

$$ \begin{aligned} \tilde{\mathbf{y}}_k &= \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} \\ \tilde{\mathbf{y}}_k &= \mathbf{H}_k \tilde{\mathbf{x}}_{k|k-1} + \mathbf{v}_k \end{aligned} \quad (15) $$

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:

$$ \hat{\mathbf{R}}_{k|k-1} = \hat{\mathbf{R}}_{k-1}. $$

To compute the time update state estimation error, the difference $\mathbf{R}_k - \hat{\mathbf{R}}_{k|k-1}$ has no physical meaning, instead we define the time update state estimation error $\eta_{k|k-1}$ and the state estimation error $\eta_k$ as follows:

$$ \eta_{k|k-1} = \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{R}_k, \quad (16) $$

$$ \eta_k = \hat{\mathbf{R}}_k^{-1} \mathbf{R}_k. \quad (17) $$

Both $\eta_{k|k-1}$ and $\eta_k$ belong to SO(3), so they can be mapped with $\xi_{k|k-1}, \xi_{k-1} \in \mathbb{R}^3$ using the exponential map $\exp_m(\cdot)$ as defined in (3):

$$ \eta_{k|k-1} = \exp_m(\xi_{k|k-1}), \quad (18) $$

$$ \eta_k = \exp_m(\xi_k). \quad (19) $$

By using (16) and (18) we get:

$$ \exp_m(\xi_{k|k-1}) = \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{R}_k. \quad (20) $$

Employing the model (8) we get:

$$ \exp_m(\xi_{k|k-1}) = \hat{\mathbf{R}}_{k-1}^{-1} \mathbf{R}_{k-1} \exp_m(\omega_{k-1} + \mathbf{w}_{k-1}), $$

Then based on (19):

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

Applying the approximation (7) gives:

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

The innovation can be written such as:

$$ \tilde{\mathbf{Y}}_k = \mathbf{Y}_k - h(\hat{\mathbf{R}}_{k|k-1}), $$

$$ \tilde{\mathbf{Y}}_k = \left( \begin{array}{c} \mathbf{R}_k^{-1} \mathbf{g} \\ \mathbf{R}_k^{-1} \mathbf{m}_e \end{array} \right) + \mathbf{v}_k^y - \left( \begin{array}{c} \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g} \\ \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e \end{array} \right), $$

$$ \tilde{\mathbf{Y}}_k = \left( \begin{array}{c} \mathbf{R}_k^{-1} \mathbf{g} - \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g} \\ \mathbf{R}_k^{-1} \mathbf{m}_e - \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e \end{array} \right) + \mathbf{v}_k^y. $$

By substituting $\mathbf{R}_k$ with (20), it follows successively:

$$ \tilde{\mathbf{Y}}_k = \left( \begin{array}{c} \exp_m(-\xi_{k|k-1}) \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g} - \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g} \\ \exp_m(-\xi_{k|k-1}) \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e - \hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e \end{array} \right) + \mathbf{v}_k^y, $$

$$ \tilde{\mathbf{Y}}_k = \left( \begin{array}{c} (\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{array} \right) + \mathbf{v}_k^y. $$

Using the first-order approximation (6), we obtain:

$$ \tilde{\mathbf{Y}}_k = \left( \begin{array}{c} -(\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{array} \right) + \mathbf{v}_k^y. $$

Finally, using the useful property (2), we obtain:

$$ \tilde{\mathbf{Y}}_k = \left( \begin{array}{c} (\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{array} \right) + \mathbf{v}_k^y, $$

$$ \tilde{\mathbf{Y}}_k = \left( \begin{array}{c} (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g}) \times \\ (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e) \times \end{array} \right) \xi_{k|k-1} + \mathbf{v}_k^y. \quad (22) $$

Equations (21) and (22) are the linearized estimation error system in $\mathbb{R}^3$ akin to (14) and (15). The linearized estimation error system has the matrices:

$$ \left\{ \begin{array}{l} \mathbf{A}_k^l = \mathbf{I}_3 \\ \mathbf{E}_k^l = \mathbf{I}_3 \\ \mathbf{H}_k^l = \left( \begin{array}{c} (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g}) \times \\ (\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{m}_e) \times \end{array} \right) \end{array} \right. \quad \forall k \in \mathbb{N}^*. \quad (23) $$

Hence the conventional RTSKF with unknown input can be applied to derive the gains $\mathbf{K}_k^x, \mathbf{K}_k^d, \mathbf{V}_k$ and the updated covariances (after verifying that conditions (13) are satisfied). The terms $\mathbf{K}_k^x \tilde{\mathbf{y}}_k$ and $\mathbf{V}_k \mathbf{d}_k$ 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: $\bar{\mathbf{R}}_k = \hat{\mathbf{R}}_{k|k-1} \exp_m(\mathbf{K}_k^x \tilde{\mathbf{Y}}_k)$, $\hat{\mathbf{R}}_k = \bar{\mathbf{R}}_k \exp_m(\mathbf{V}_k \mathbf{d}_k)$.

Let us now check if the matrices in (23) satisfy the conditions (13), the vectors $\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{g}$ and $\hat{\mathbf{R}}_{k|k-1}^{-1} \mathbf{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 $\mathrm{rank}[\mathbf{H}_k^l] = 3$, and also we have $\mathrm{rank}[\mathbf{E}_k^l] = \mathrm{rank}[\mathbf{I}_3] = 3$ and $\mathrm{rank}[\mathbf{H}_k^l \mathbf{E}_k^{l*}] = \mathrm{rank}[\mathbf{H}_k^l] = 3$. Then the conditions (13) are satisfied.

It is important to note that matrices $\mathbf{A}_k^l$ and $\mathbf{E}_k^l$ are independent of the estimation, but the matrix $\mathbf{H}_k^l$ is not. However, an approximation is used to obtain the innovation covariance matrix $\mathrm{Cov}(\tilde{\mathbf{Y}}_k) = \mathbf{H}_k^l \mathrm{Cov}(\xi_{k|k-1}) \mathbf{H}_k^{l*} + \mathcal{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.

<caption>**Algorithm 2** Proposed RTSKF-SO(3) for attitude estimation with unknown angular velocities</caption>

Input: $\hat{\mathbf{R}}_{k-1}, \mathbf{P}_k^x, \mathbf{Y}_k = [\mathbf{a}_k^b; \mathbf{B}_k^b]$

1: $\hat{\mathbf{R}}_{k|k-1} = \hat{\mathbf{R}}_{k-1}$

2: $\mathbf{P}_k^x = \mathbf{P}_k^{x-1} + \mathcal{Q}_{k-1}$

3: $\tilde{\mathbf{Y}}_k = \mathbf{Y}_k - h(\hat{\mathbf{R}}_{k|k-1})$

4: $\mathbf{C}_k = \mathbf{H}_k^l \mathbf{P}_k^x \mathbf{H}_k^{l*} + \mathcal{R}_k$

5: $\mathbf{K}_k^x = \mathbf{P}_k^x \mathbf{H}_k^{l*} \mathbf{C}_k^{-1}$

6: $\bar{\mathbf{R}}_k = \hat{\mathbf{R}}_{k|k-1} \exp_m(\mathbf{K}_k^x \tilde{\mathbf{Y}}_k)$

7: $\mathbf{P}_k^x = (\mathbf{I} - \mathbf{K}_k^x \mathbf{H}_k^l) \mathbf{P}_k^x \mathbf{H}_k^{l*}$

8: $\mathbf{P}_k^d = (\mathbf{H}_k^l \mathbf{C}_k^{-1} \mathbf{H}_k^l)^{-1}$

9: $\mathbf{K}_k^d = \mathbf{P}_k^d \mathbf{H}_k^{l*} \mathbf{C}_k^{-1}$

10: $\hat{\mathbf{d}}_k = \mathbf{K}_k^d \tilde{\mathbf{Y}}_k$

11: $\mathbf{V}_k = \mathbf{I} - \mathbf{K}_k^x \mathbf{H}_k^l$

12: $\hat{\mathbf{R}}_k = \bar{\mathbf{R}}_k \exp_m(\mathbf{V}_k \hat{\mathbf{d}}_k)$

13: $\mathbf{P}_k^x = \mathbf{P}_k^x + \mathbf{V}_k \mathbf{P}_k^d \mathbf{V}_k^{l*}$

14: return $\mathbf{R}_k, \mathbf{P}_k^x$
# V. SIMULATIONS OF THE RTSKF-SO(3) ESTIMATION: SETUP AND MAIN RESULTS

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.

<table><caption>TABLE I: Implemented algorithms in simulation with corresponding sensor usage (Acc: 3-axis accelerometer, Mag: 3-axis magnetometer, Gyro: 3-axis gyroscope)</caption><thead><tr><th>Algorithm</th><th>Acc</th><th>Mag</th><th>Gyro</th><th>Current consumptions</th></tr></thead><tbody><tr><td>RTSKF-SO(3)</td><td>✓</td><td>✓</td><td></td><td>Microampere range</td></tr><tr><td>TRIAD</td><td>✓</td><td>✓</td><td></td><td>Microampere range</td></tr><tr><td>IEKF</td><td>✓</td><td>✓</td><td>✓</td><td>Milliampere range</td></tr></tbody></table>

## A. Setup of the simulation

The system was simulated for 100 sec for each Monte Carlo iteration with a sampling time of $\Delta T = 0.01$ sec (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 $\mathbf{g} = [0, 0, 9.81]^T \text{ m/s}^2$ and $\mathbf{m}_e = [0.23, 0.01, 0.41]^T \text{ 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^\circ, 45^\circ, 45^\circ]$ 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

<table><caption>TABLE II: The true angular velocity used in simulation</caption><thead><tr><th></th><th>0 ≤ t < 50</th><th>50 ≤ t ≤ 100</th></tr></thead><tbody><tr><td>ω<sub>x</sub></td><td>0.8 cos(1.2t)</td><td>− cos(1.2t)</td></tr><tr><td>ω<sub>y</sub></td><td>−1.1 cos(0.5t)</td><td>0.5 cos(0.8t)</td></tr><tr><td>ω<sub>z</sub></td><td>−0.4 cos(0.3t)</td><td>−0.7 cos(0.7t)</td></tr></tbody></table>

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 $\sigma_a = 0.01 \text{ m/sec}^2$ and $\sigma_m = 0.005 \text{ 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] sec, where we start computing the RMSE after 2 sec 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] sec, while the right part corresponds to the time range of [0.2, 100] sec. In the left part, it is clear that RTSKF-SO(3) achieves convergence quickly, specifically in less than 0.05 sec. 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. These findings indicate that the RTSKF-SO(3) estimation method performs well and provides good results even if the angular velocity is unknown.

<figure><img src="image_2.png" /><figcaption>Fig. 1: RTSKF-SO(3) estimation error</figcaption></figure>

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.
<table><caption>TABLE III: RMSE (in degrees) for both RTSKF-SO(3) and TRIAD</caption><thead><tr><td>TRIAD RMSE (degrees)</td><td>RTSKF-SO(3) RMSE (degrees)</td></tr></thead><tbody><tr><td>0.73</td><td>0.58</td></tr></tbody></table>

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 $\sigma_{\omega}$). 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).

<table><caption>TABLE IV: RMSE (in degrees) for both RTSKF-SO(3) and IEKF for various gyroscope noise level</caption><thead><tr><th>σ<sub>ω</sub><br>(rad/sec)</th><th>IEKF<br>RMSE (degrees)</th><th>RTSKF-SO(3)<br>RMSE (degrees)</th></tr></thead><tbody><tr><td>0.01</td><td>0.35</td><td rowspan="3">0.58</td></tr><tr><td>0.05</td><td>0.60</td></tr><tr><td>0.10</td><td>0.71</td></tr></tbody></table>

## 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. This would further enhance the applicability of the algorithm in practical situations.

## REFERENCES

[1] Xiaoping Yun, Eric R. Bachmann, and Robert B. McGhee. A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements. *IEEE Transactions on Instrumentation and Measurement*, 57(3):638–650, 2008.

[2] Malcolm David Shuster and S D_Oh. Three-axis attitude determination from vector observations. *Journal of Guidance and Control*, 4(1):70–77, 1981.

[3] Hassen Fourati and Djamel Eddine Chouaib Belkhiat, editors. *Multisensor Attitude Estimation*. CRC Press, November 2016.

[4] Axel Barrau and Silvère Bonnabel. A mathematical framework for IMU error propagation with applications to preintegration. In *2020 IEEE International Conference on Robotics and Automation (ICRA)*, pages 5732–5738, Paris, France, 2020.

[5] Axel Barrau and Silvère Bonnabel. Invariant Kalman filtering. *Annual Review of Control, Robotics, and Autonomous Systems*, 1(1):237–257, 2018.

[6] Qi Liu, James Williamson, Kun Li, Wyatt Mohrman, Qin Lv, Robert P. Dick, and Li Shang. Gazelle: Energy-efficient wearable analysis for running. *IEEE Transactions on Mobile Computing*, 16(9):2531–2544, 2017.

[7] Yi-Lung Tsai, Ting-Ting Tu, Hyeoungho Bae, and Pai H. Chou. EcoIMU: A dual triaxial-accelerometer inertial measurement unit for wearable applications. In *2010 International Conference on Body Sensor Networks*, pages 207–212, Singapore, 2010.

[8] Aida Makni, Hassen Fourati, and Alain Y. Kibangou. Energy-aware adaptive attitude estimation under external acceleration for pedestrian navigation. *IEEE/ASME Transactions on Mechatronics*, 21(3):1366–1375, 2016.

[9] Tobias Hiller, Zsigmond Pentek, Jan-Timo Liewald, Alexander Buhmann, and Hubert Roth. Origins and mechanisms of bias instability noise in a three-axis mode-matched MEMS gyroscope. *Journal of Microelectromechanical Systems*, 28(4):586–596, 2019.

[10] Padgaonkar AJ, Krieger KW, and King AI. Measurement of Angular Acceleration of a Rigid Body Using Linear Accelerometers. *Journal of Applied Mechanics*, 42(3):552–556, 09 1975.

[11] Chin-Woo Tan, Sungsu Park, Kirill Mostov, and Pravin Varaiya. Design of gyroscope-free navigation systems. In *ITSC 2001. 2001 IEEE Intelligent Transportation Systems. Proceedings (Cat. No.01TH8585)*, pages 286–291, Oakland, CA, USA, 2001.

[12] Chien-Shu Hsieh. Robust two-stage Kalman filters for systems with unknown inputs. *IEEE Transactions on Automatic Control*, 45(12):2374–2378, 2000.

[13] Steven Gillijns and Bart De Moor. Unbiased minimum-variance input and state estimation for linear discrete-time systems. *Automatica*, 43(1):111–116, 2007.

[14] Steven Gillijns and Bart De Moor. Unbiased minimum-variance input and state estimation for linear discrete-time systems with direct feedthrough. *Automatica*, 43(5):934–937, 2007.

[15] Sze Zheng Yong, Minghui Zhu, and Emilio Frazzoli. A unified filter for simultaneous input and state estimation of linear discrete-time stochastic systems. *Automatica*, 63:321–329, 2016.

[16] Chien-Shu Hsieh. State estimation for nonlinear systems with unknown inputs. In *2012 7th IEEE Conference on Industrial Electronics and Applications (ICIEA)*, pages 1533–1538, Singapore, 2012.

[17] Axel Barrau and Silvère Bonnabel. Three examples of the stability properties of the invariant extended Kalman filter. *IFAC-PapersOnLine*, 50(1):431–437, 2017. 20th IFAC World Congress.

[18] Martin Brossard, Silvère Bonnabel, and Jean-Philippe Condomines. Unscented Kalman filtering on lie groups. In *2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, pages 2485–2491, Vancouver, BC, Canada, 2017.

[19] Axel Barrau and Silvère Bonnabel. The geometry of navigation problems. *IEEE Transactions on Automatic Control*, 68(2):689–704, 2023.

[20] Robert Grover Brown and Patrick YC Hwang. *Introduction to Random Signals and Applied Kalman Filtering with MATLAB Exercises, 4th Edition*. Wiley, 2012.

[21] Angelo M Sabatini. Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing. *IEEE Transactions on Biomedical Engineering*, 53(7):1346–1356, 2006.