# Attitude Estimation on SO(3) With Unknown Input

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, Christophe Prieur

► To cite this version:

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, Christophe Prieur. Attitude Estimation on SO(3) With Unknown Input. International Journal of Robust and Nonlinear Control, 2025, 35 (17), pp.7355-7366. 10.1002/rnc.70060 . hal-05148197

HAL Id: hal-05148197

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

Submitted on 7 Jul 2025

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 Attribution 4.0 International License
# Attitude Estimation on SO(3) with Unknown Input

Ghadeer Shaaban¹ | Hassen Fourati¹ | Alain Kibangou¹,² | Christophe Prieur¹

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

²Faculty of Science, University of Johannesburg (Auckland Park Campus), Johannesburg, South Africa

## Abstract

State estimation in the presence of unknown input presents a significant challenge in many domains. Few works in the literature have addressed this problem when the state belongs to the special orthogonal group SO(3), which is not a vector space. This paper proposes a state estimation algorithm designed for SO(3) with unknown input, specifically for cases where the unknown input affects the state dynamics without direct feedthrough to the output, with a proof of local optimality and local stability, which has not been explored yet. Finally, this paper demonstrates an application in gyro-free attitude estimation, using only measurements from a 3-axis accelerometer and a 3-axis magnetometer, by considering the angular velocity from a 3-axis gyroscope as an unknown input. Monte Carlo simulations show better accuracy and performance over existing attitude estimation methods using only accelerometer and magnetometer sensors.

## KEYWORDS

State estimation, minimum variance estimation, unknown input, SO(3), attitude estimation, gyro-free.

## 1 | INTRODUCTION

State estimation using dynamic models and observations has long been of interest to scientists and engineers across various domains. One particular problem in this field is the estimation of the state in the presence of unknown input, where no prior information about the unknown input is available, also known as the unknown input filtering (UIF) problem. Extensive research has focused on designing state estimators for linear systems with unknown input, aiming to minimize the trace of the state estimation error covariance matrix while ensuring unbiasedness. Notably, several works have contributed to the linear CIF problem when the unknown input affects the state dynamics without direct feedthrough to the output, see [14, 18]. Extensions for linear systems with a full rank direct feedthrough matrix have also been proposed in [15, 32]. A unified filter for linear systems was proposed in [37, 38] with no restriction on the direct feedthrough matrix.

Researchers have made efforts to extend CIF algorithms to nonlinear systems. Inspired by nonlinear versions of the Kalman filter: the extended Kalman filter (EKF), and the unscented Kalman filter (UKF), the authors in [19] propose two distinct approaches for state estimation for nonlinear systems with unknown input. The first approach involves linearizing around the current state estimate, and the second involves generating a set of sample points, known as sigma points, and propagating them through the system dynamics. Several other extensions have been addressed in the literature, such as [11] inspired by the Bayesian estimation framework, and [12] inspired by the ensemble Kalman filter (EnKF).

The works mentioned above focus on state estimation with unknown input where the states belong to a vector space. In this paper, our objective is to address the problem of state estimation with unknown input for systems with states lying on the special orthogonal group SO(3) of rotations. Significant attention is given to estimation and filtering for systems on SO(3) when full input information is available for attitude estimation, some of these works are designed in [1, 4, 5]. The motivation for using the SO(3) group for attitude estimation comes from the fact that the SO(3) group provides a structured mathematical framework for representing three-dimensional rotations. Attitude estimation using the mathematical representations on SO(3) is often preferred in the literature over other representations [5, 16, 36], such as Euler angles and quaternions, due to the latter's well-known drawbacks. The utilization of Euler angles representation introduces the issue of singularity [13]. Using quaternion for attitude estimation with EKF is popular in the literature because it is an easy-to-implement method [26], however, normalization of the quaternion is required at each time step, which can further affect the quality of estimation.

Attitude estimation plays a critical role in various applications, including spacecraft control, aerial navigation, and robotics, by ensuring precise stabilization and fault tolerance. In spacecraft attitude control, advanced estimation strategies, such as intermediate state observers, have been developed to address challenges like model parameter uncertainty, actuator faults, and external disturbances. For example, a stochastically intermediate observer to estimate attitude and fault signals simultaneously
under uncertainty has been introduced in [22]. Additionally, a hybrid nonfragile observer-based strategy for vibration suppression in flexible spacecraft has been proposed in [23]. These studies demonstrate the importance of attitude estimation methods in enhancing spacecraft control performance under complex conditions.

To the best of the authors' knowledge, only a few works in the literature address the problem of estimation on SO(3) with unknown input. Attitude estimation on SO(3) under unknown dynamic disturbances has been investigated in [36], while the problem of attitude estimation in the presence of randomly occurring false data injection attacks has been addressed in [30]. In [28, 29], attitude estimation on SO(3) under unknown external acceleration is designed, treating it as an unknown input with direct feedthrough to the output. Attitude estimation on SO(3) when angular velocity measurements contain unknown bias is addressed in several works, such as [4, 16, 21]. Recently, the work [27] designs an algorithm, called RTSKF-SO(3), where the full information of angular velocity is considered as unknown input affecting the state dynamic without direct feedthrough to the output. The algorithm RTSKF-SO(3) is designed for the application of gyro-free attitude estimation, relying only on measurements from two 3-axis sensors: magnetometer and accelerometer. Gyro-free attitude estimation took the attention of several works in the literature [33, 34], due to the drawbacks of using gyroscopes, such as power consumption [24, 34], and their vulnerability to drift and bias [17]. The recent algorithm RTSKF-SO(3) [27] showed better estimation accuracy than the existing algorithms that use only measurements from the accelerometer and magnetometer, which are the static algorithms. A static algorithm doesn't depend on the rotation dynamic model or angular velocity measurements, instead, it uses the measurements of acceleration and the magnetic field at each time step to reconstruct the attitude. Some known static algorithms are FQA [39], TRIAD [31], and QUEST [31]. The problem of determining the attitude of a rigid body based on the measurements of two independent vectors is known as Wahba's problem [35]. We refer to the survey [10] for a detailed review of existing attitude estimation methods.

The RTSKF-SO(3) is compared with a static algorithm, namely TRIAD [31], since there are no dynamic algorithms available that exclusively utilize magnetometer and accelerometer measurements without depending on angular velocity measurements. The improved accuracy of RTSKF-SO(3) compared to the static algorithm (e.g. TRIAD) is due to its recursive estimation process. It utilizes previous attitude estimates, their error covariance matrices, and current accelerometer and magnetometer measurements combined with their noise covariance matrices, exploiting the attitude's dynamic model. In contrast, static algorithms rely only on the current measurements at each step, without considering prior estimates or the system's dynamics. The design of RTSKF-SO(3) was based on extending the robust two-stage Kalman filters in [18]. However, the local optimality of RTSKF-SO(3) in terms of minimum variance under unbiasedness conditions is not guaranteed nor studied. Additionally, RTSKF-SO(3) deals specifically with the case where the outputs are measurements from two specific sensors: a 3-axis accelerometer and a 3-axis magnetometer. Developing this algorithm for general nonlinear output functions, rather than limiting it to accelerometer and magnetometer measurements, is important for various applications. This expansion allows for potential applications in navigation with data from other sources such as cameras. In some works for attitude estimation, where the gyroscope measurements are given, the output function contains landmarks position obtained using vision data [8].

In Kalman filtering on SO(3), local optimality and stability are studied instead of global ones due to the first-order approximation of the estimation error [2, 3]. Local optimality is achieved by minimizing the estimation error covariance matrix at the first-order approximation level, while local stability is demonstrated by ensuring that the estimation error remains bounded. Previous works have typically focused on local stability in the deterministic case (i.e., without noise) [2], and local stability under noise has only been proven for specific application examples [3]. In this paper, we demonstrate both local optimality and stability under noisy conditions for our proposed state estimation algorithm.

The main contributions of this paper can be summarized as follows:

* Designing a local optimal state estimation algorithm with unknown input for systems on SO(3) when the unknown input affects the state dynamic and has no direct feedthrough to the output, and proving its local optimality.

* This algorithm is designed for systems with a general nonlinear output function, where we derive the sufficient conditions that the output function must satisfy for local optimality.

* A concrete application of the proposed algorithm is presented, which is gyro-free attitude estimation when the unknown input is angular velocity and outputs are measurements from a 3-axis accelerometer and a 3-axis magnetometer. The proposed algorithm outperformed existing algorithms for gyro-free attitude estimation.

The remainder of this paper is structured as follows: Section 2 provides a concise mathematical background. Section 3 gives the model and the problem statement. Section 4 presents the filter equations and outlines the algorithm's steps. Section 5 provides a comprehensive derivation of the algorithm and studies its stability. Section 6 offers an example of the algorithm's
application, specifically gyro-free attitude estimation, and compares it with existing solutions in the literature, by conducting Monte Carlo simulations, and by using real datasets, ensuring the reliability of our findings. Finally, the paper ends with a conclusion in Section 7.

## 2 | BACKGROUND AND PRELIMINARIES

In this paper, we denote matrices, vectors, and scalars using bold upper-case, bold lower-case, and regular lower-case symbols, respectively. The symbols det(.), E(.), tr(.), $\hat{x}$, $\mathbf{I}_n$, and $\mathbf{0}$ represent the determinant operator, the expected value operator, the trace operator, the estimate of a variable $x$, the identity matrix of order $n$, and the all-zero matrix, respectively.

The orientation between the fixed frame (navigation frame) and the body frame attached to a moving rigid body can be represented as a rotation matrix that belongs to the special orthogonal group [25]:

$$ \mathrm{SO}(3) = \{ \mathbf{R} \in \mathbb{R}^{3 \times 3} : \mathbf{R}\mathbf{R}^T = \mathbf{I}_3, \det(\mathbf{R}) = 1 \}. \quad (1) $$

Consider a vector written in the body frame, denoted as $\mathbf{v}^b \in \mathbb{R}^3$, and the same vector written in the navigation frame, denoted as $\mathbf{v}^n \in \mathbb{R}^3$, the relationship between the two representations is given by $\mathbf{v}^n = \mathbf{R}\mathbf{v}^b$. A set of Euler angles [$\phi, \theta, \psi$], where $\phi, \theta$, and $\psi$ represent the roll, pitch, and yaw angles, respectively, describe the three sequential rotations. Several Euler angle configurations exist, and the typical convention in the literature for attitude representation is the XYZ configuration [26]. Using this convention, the rotation matrix $\mathbf{R}$ is expressed as [13, Chapter 3.3]:

$$ \mathbf{R} = \mathbf{R}_z(\psi)\mathbf{R}_y(\theta)\mathbf{R}_x(\phi), $$

where:

$$ \mathbf{R}_z(\psi) = \begin{pmatrix} \cos(\psi) & -\sin(\psi) & 0 \\ \sin(\psi) & \cos(\psi) & 0 \\ 0 & 0 & 1 \end{pmatrix}, \quad \mathbf{R}_y(\theta) = \begin{pmatrix} \cos(\theta) & 0 & \sin(\theta) \\ 0 & 1 & 0 \\ -\sin(\theta) & 0 & \cos(\theta) \end{pmatrix}, \quad \mathbf{R}_x(\phi) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos(\phi) & -\sin(\phi) \\ 0 & \sin(\phi) & \cos(\phi) \end{pmatrix}. $$

A vector $\xi \in \mathbb{R}^3$ can be mapped to an element $\mathbf{R} \in \mathrm{SO}(3)$ through the exponential map $\exp_m : \mathbb{R}^3 \to \mathrm{SO}(3)$, accordingly to the formula [1]:

$$ \exp_m(\xi) = \exp((\xi)_\times) $$

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

We express the exponential map as a power series:

$$ \exp_m(\xi) = \exp((\xi)_\times) = \sum_{i=0}^{\infty} \frac{(\xi)_\times^i}{i!}, $$

and define the normalized vector as $\mathbf{u} = \frac{\xi}{\|\xi\|}$ (when $\|\xi\| \neq 0$). To derive an explicit expression for the exponential map, we write the powers of the skew-symmetric matrix of the normalized vector as:

$$ (\mathbf{u})_\times^0 = \mathbf{I}, \quad (\mathbf{u})_\times^2 = \mathbf{u}\mathbf{u}^T - \mathbf{I}, \quad (\mathbf{u})_\times^3 = -(\mathbf{u})_\times. $$

Using these properties, the exponential map can be written as:

$$ \exp_m(\xi) = \mathbf{I} + \left( \|\xi\| - \frac{\|\xi\|^3}{3!} + \frac{\|\xi\|^5}{5!} + \cdots \right) (\mathbf{u})_\times + \left( \frac{\|\xi\|^2}{2!} - \frac{\|\xi\|^4}{4!} + \frac{\|\xi\|^6}{6!} + \cdots \right) (\mathbf{u})_\times^2, $$

where we recognize the series expansions of $\sin(\|\xi\|)$ and $\cos(\|\xi\|)$, yielding the following expression:
$$ \left\{ \begin{array}{ll} \xi = 0, & \exp_m(\xi) = I_3, \\ \forall \xi \in \mathbb{R}^3 \setminus \{0\}, \quad \exp_m(\xi) = I_3 + \frac{\sin(\|\xi\|)}{\|\xi\|} (\xi)_\times + 2 \frac{\sin(\|\xi\|/2)}{\|\xi\|^2} (\xi)_\times, \end{array} \right. \qquad (3) $$

The following useful properties of the exponential map and the skew-symmetric matrix are used in the paper. Letting $\xi_1, \xi_2 \in \mathbb{R}^3$, we have [5]:

$$ \exp_m(\xi_1)^{-1} = \exp_m(-\xi_1), \qquad (4) $$

$$ (\xi_1)_\times \xi_2 = -(\xi_2)_\times \xi_1, \qquad (5) $$

and we have the first-order approximation [5]:

$$ \exp_m(\xi_1) = I_3 + (\xi_1)_\times + \mathcal{O}(\|\xi_1\|^2), \qquad (6) $$

$$ \exp_m(\xi_1) \exp_m(\xi_2) = \exp_m(\xi_1 + \xi_2 + \mathcal{O}(\|\xi_1\|^2, \|\xi_2\|^2)), \qquad (7) $$

where $\mathcal{O}(\cdot)$ denotes the higher-order terms beyond the considered approximation. Equation (7) represents the Baker-Campbell-Hausdorff (BCH) formula, which is widely used in the design of Kalman filters on SO(3) [7].

The estimation error on SO(3) can be defined as $\hat{\mathbf{R}}^{-1}\mathbf{R} \in \text{SO}(3)$ [5], where $\mathbf{R}, \hat{\mathbf{R}} \in \text{SO}(3)$ are the rotation matrix state and its estimate. This estimation error can be mapped to a vector $\xi \in \mathbb{R}^3$ using the inverse of the exponential map.

In the following, Lemma 1 provides the rank property of vertically concatenated two three-by-three skew-symmetric matrices and Theorem 1 is the Gauss-Markov Theorem. The proofs of Lemma 1 and Theorem 1 are available in [27] and [20, Chapter 3.4.2], respectively.

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

**Theorem 1.** 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 $\mathcal{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\mathcal{R}_v^{-1}\mathbf{D}_x)^{-1}\mathbf{D}_x^T\mathcal{R}_v^{-1}\mathbf{y}$.

## 3 | PROBLEM STATEMENT

The attitude discrete-time model of a rigid body is given by [1]:

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

$$ \mathbf{y}_k = h(\mathbf{R}_k) + \mathbf{v}_k, \qquad (9) $$

where, for any time step $k \in \mathbb{N}$, $\mathbf{R}_k$ is the attitude state taking values on SO(3), $\omega_k \in \mathbb{R}^3$ is an unknown input vector, where $\omega_k = \Delta T \omega_k^{\text{true}}$ and $\omega_k^{\text{true}} \in \mathbb{R}^3$ is the unknown true angular velocity and $\Delta T$ is the sampling time. The measurement vector is denoted by $\mathbf{y}_k \in \mathbb{R}^m$. The function $h: \text{SO}(3) \to \mathbb{R}^m$ is the observation function. The measurement noise $\mathbf{v}_k \in \mathbb{R}^m$ is assumed to be an uncorrelated zero-mean white random signal with positive definite covariance matrix $\mathcal{R}_k$.

**Remark:** The dynamic model (8) is a kinematic model, for which the attitude state evolves only because of the input angular velocity $\omega_k^{\text{true}}$, so it doesn't contain process noise.

The objective of this paper is to design a filter that estimates $\mathbf{R}_k$, without any prior knowledge or assumptions about the unknown input $(\omega_k)_{i \in \mathbb{N}}$, using only the sequence of the measurements $\{\mathbf{y}_0, \mathbf{y}_1, ..., \mathbf{y}_k\}$. For this purpose, a novel recursive algorithm named UMV-SO(3), is designed. It gives an unbiased minimum variance first-order approximation-based estimator of $\mathbf{R}_k$ based on an unbiased estimate of $\mathbf{R}_{k-1}$, and the measurements $\mathbf{y}_k$.

## 4 | UMV-SO(3) DESIGN

The unbiased minimum variance state estimation for linear discrete-time systems with unknown input was introduced in [14], where a four-step recursive filter was designed. The first step gives a biased prediction of the state due to the unknown input, based on an unbiased state estimation in the previous time step and the dynamic model. The second step gives an unbiased
minimum variance estimate of the unknown input using the measurements. The third step uses the unbiased estimate of the unknown input to obtain an unbiased prior estimation of the state. Finally, the fourth step minimizes the state estimation variance using a correction similar to the Kalman filter. Based on the same principle, we propose the following equations of the UMV-SO(3):

$$ \hat{\mathbf{R}}_{klk-1} = \hat{\mathbf{R}}_{k-1} \qquad (10) $$

$$ \hat{\omega}_{k-1} = \mathbf{M}_k \left( \mathbf{y}_k - h \left( \hat{\mathbf{R}}_{klk-1} \right) \right) \qquad (11) $$

$$ \hat{\mathbf{R}}_{k}^{*} = \hat{\mathbf{R}}_{klk-1} \exp_{m}(\hat{\omega}_{k-1}) \qquad (12) $$

$$ \hat{\mathbf{R}}_{k} = \hat{\mathbf{R}}_{k}^{*} \exp_{m} \left( \mathbf{K}_{k} \left( \mathbf{y}_{k} - h \left( \hat{\mathbf{R}}_{k}^{*} \right) \right) \right) \qquad (13) $$

where the matrices $\mathbf{M}_k \in \mathbb{R}^{3 \times m}$, and $\mathbf{K}_k \in \mathbb{R}^{3 \times m}$ 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}$, 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}$. Algorithm 1 presents the detailed steps of the proposed recursive filter UMV-SO(3).

**Algorithm 1 UMV-SO(3)**

Require: $\hat{\mathbf{R}}_{k-1}$, $\mathbf{P}_{k-1}$, $\mathbf{y}_k$

$$ \text{1:} \quad \hat{\mathbf{R}}_{klk-1} = \hat{\mathbf{R}}_{k-1} $$

$$ \text{2:} \quad \mathbf{H}_k = \left. \frac{\partial h \left( \hat{\mathbf{R}}_{klk-1} \exp_m (\boldsymbol{\xi}) \right)}{\partial \boldsymbol{\xi}} \right|_{\boldsymbol{\xi}=0} $$

$$ \text{3:} \quad \tilde{\mathcal{R}}_k = \mathbf{H}_k \mathbf{P}_{k-1} \mathbf{H}_k^T + \mathcal{R}_k $$

$$ \text{4:} \quad \mathbf{M}_k = (\mathbf{H}_k^T \tilde{\mathcal{R}}_k^{-1} \mathbf{H}_k)^{-1} \mathbf{H}_k^T \tilde{\mathcal{R}}_k^{-1} $$

$$ \text{5:} \quad \hat{\omega}_{k-1} = \mathbf{M}_k (\mathbf{y}_k - h(\hat{\mathbf{R}}_{klk-1})) $$

$$ \text{6:} \quad \hat{\mathbf{R}}_{k}^{*} = \hat{\mathbf{R}}_{klk-1} \exp_{m}(\hat{\omega}_{k-1}) $$

$$ \text{7:} \quad \mathbf{H}_k^* = \left. \frac{\partial h \left( \hat{\mathbf{R}}_k^* \exp_m (\boldsymbol{\xi}) \right)}{\partial \boldsymbol{\xi}} \right|_{\boldsymbol{\xi}=0} $$

$$ \text{8:} \quad \mathcal{R}_k^* = (\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k) \mathcal{R}_k (\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k)^T $$

$$ \text{9:} \quad \mathbf{S}_k^* = \mathbf{M}_k \mathcal{R}_k (\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k)^T $$

$$ \text{10:} \quad \Gamma_k = [\gamma_{k1} \ \gamma_{k2} \ \dots \ \gamma_{kp_k}]^T \triangleright \text{where } (\gamma_{ki})_{i \in \{1, \dots, p_k\}} \text{ are the } p_k \text{ linearly independent eigenvectors corresponding to the } p_k \text{ non-zero eigenvalues of the matrix } \mathcal{R}_k^*. $$

$$ \text{11:} \quad \mathbf{K}_k = -\mathbf{S}_k^* \Gamma_k^T (\Gamma_k \mathcal{R}_k^* \Gamma_k^T)^{-1} \Gamma_k $$

$$ \text{12:} \quad \hat{\mathbf{R}}_k = \hat{\mathbf{R}}_k^* \exp_m (\mathbf{K}_k (\mathbf{y}_k - h(\hat{\mathbf{R}}_k^*))) $$

$$ \text{13:} \quad \mathbf{P}_k = \mathbf{K}_k \mathbf{S}_k^{*T} + \mathbf{M}_k \mathcal{R}_k \mathbf{M}_k^T $$

$$ \text{14:} \quad \text{Return: } \hat{\mathbf{R}}_k, \mathbf{P}_k $$

In the next section, considering $\hat{\mathbf{R}}_{k-1}$ as unbiased, and the matrix $\mathbf{H}_k$ in the second step of Algorithm 1 has full column rank, we show that the first-order approximation-based estimators of $\hat{\omega}_{k-1}$ and $\hat{\mathbf{R}}_k$ given by Algorithm 1 are unbiased minimum variance (see Theorem 2 and Theorem 3).

## 5 | UMV-SO(3) ANALYSIS

This section presents a comprehensive derivation of Algorithm 1. It is divided into four subsections, each addressing one of the filter's equations. The fifth subsection studies its stability. We define the prediction error $\exp_m(\xi_{klk-1}) = \hat{\mathbf{R}}_{klk-1}^{-1} \mathbf{R}_k$, the prior estimation error $\exp_m(\xi_k^*) = \hat{\mathbf{R}}_k^{-1} \mathbf{R}_k$, and the estimation error be $\exp_m(\xi_k) = \hat{\mathbf{R}}_k^{-1} \mathbf{R}_k$, with the corresponding covariance matrices $\mathbf{P}_{klk-1} = E(\xi_{klk-1} \xi_{klk-1}^T)$, $\mathbf{P}_k^* = E(\xi_k^* \xi_k^{*T})$, and $\mathbf{P}_k = E(\xi_k \xi_k^{*T})$, respectively.
## 5.1 | Prediction error

Lemma 2 considers the prediction (10) and finds the prediction error equation to be utilized in the subsequent subsections.

**Lemma 2.** Let $\hat{\mathbf{R}}_{k-1}$ be unbiased, then the first-order approximation-based predictor (10) has prediction error

$$ \xi_{klk-1} = \xi_{k-1} + \omega_{k-1}, \qquad (14) $$

with expected value $E(\xi_{klk-1}) = \omega_{k-1}$.

*Proof.* Starting with the prediction error equation $\exp_m(\xi_{klk-1}) = \hat{\mathbf{R}}_{klk-1}^{-1} \mathbf{R}_k$, then employing the rotation dynamic (8) and the filter's first step (10) yields:

$$ \begin{aligned} \exp_m(\xi_{klk-1}) &= \hat{\mathbf{R}}_{k-1}^{-1} \mathbf{R}_{k-1} \exp_m(\omega_{k-1}), \\ &= \exp_m(\xi_{k-1}) \exp_m(\omega_{k-1}). \end{aligned} $$

Applying the BCH approximation (7) gives $\xi_{klk-1} = \xi_{k-1} + \omega_{k-1}$, and yields $E(\xi_{klk-1}) = \omega_{k-1}$, since $E(\xi_{k-1}) = \mathbf{0}$ (unbiased $\mathbf{R}_{k-1}$).

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

## 5.2 | Unknown input estimation

This subsection addresses the unknown input estimator (11), where Theorem 2 gives the sufficient conditions of its local optimality, with the help of Lemma 3 and Lemma 4.

We define $\mathbf{H}_k$ which is the Jacobian of the observation function with respect to the prediction error $\mathbf{H}_k = \frac{\partial h(\hat{\mathbf{R}}_{klk-1} \exp_m(\xi))}{\partial \xi}|_{\xi=\mathbf{0}}$, then we define variable $\mathbf{e}_k$:

$$ \mathbf{e}_k = \mathbf{H}_k \xi_{k-1} + \mathbf{v}_k, \qquad (15) $$

The expected value of $\mathbf{e}_k$ is $E(\mathbf{e}_k) = \mathbf{0}$ since $E(\xi_{k-1}) = \mathbf{0}$ (unbiased $\mathbf{R}_{k-1}$), and $E(\mathbf{v}_k) = \mathbf{0}$ (zero mean measurement noise). The covariance matrix of $\mathbf{e}_k$ is given by:

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

**Lemma 3.** Let $\mathbf{R}_{k-1}$ be unbiased, then the innovation $\tilde{\mathbf{y}}_k = \mathbf{y}_k - h(\hat{\mathbf{R}}_{klk-1})$ found in the unknown input estimator (11) can be approximated as:

$$ \tilde{\mathbf{y}}_k = \mathbf{H}_k \omega_{k-1} + \mathbf{e}_k. \qquad (17) $$

*Proof.* Substituting (9) in the innovation $\mathbf{y}_k$ gives:

$$ \begin{aligned} \tilde{\mathbf{y}}_k &= h(\mathbf{R}_k) - h(\hat{\mathbf{R}}_{klk-1}) + \mathbf{v}_k, \\ &= h(\hat{\mathbf{R}}_{klk-1} \exp_m(\xi_{klk-1})) - h(\hat{\mathbf{R}}_{klk-1}) + \mathbf{v}_k. \\ &= \mathbf{H}_k \xi_{klk-1} + \mathbf{v}_k. \end{aligned} \qquad (18) $$

Lemma 2 holds, thus (14) is satisfied. Substituting (14) in (18), gives:

$$ \begin{aligned} \tilde{\mathbf{y}}_k &= \mathbf{H}_k(\xi_{k-1} + \omega_{k-1}) + \mathbf{v}_k, \\ &= \mathbf{H}_k\omega_{k-1} + \mathbf{H}_k\xi_{k-1} + \mathbf{v}_k, \\ &= \mathbf{H}_k\omega_{k-1} + \mathbf{e}_k. \end{aligned} $$

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

**Lemma 4.** Let $\mathbf{R}_{k-1}$ be unbiased, then the first-order approximation-based unknown input estimator (11) is unbiased if and only if the gain matrix $\mathbf{M}_k$ satisfies

$$ \mathbf{M}_k \mathbf{H}_k = \mathbf{I}_3, \qquad (19) $$
and thus the unknown input estimation error is:

$$ \omega_{k-1} - \hat{\omega}_{k-1} = -\mathbf{M}_k \mathbf{e}_k. \qquad (20) $$

*Proof.* Lemma 3 holds, thus (17) is satisfied, substituting it in the estimator (11) gives $\hat{\omega}_{k-1} = \mathbf{M}_k \mathbf{H}_k \omega_{k-1} + \mathbf{M}_k \mathbf{e}_k$, and thus the expected value is $E(\hat{\omega}_{k-1}) = \mathbf{M}_k \mathbf{H}_k \omega_{k-1}$ since $E(\mathbf{e}_k) = \mathbf{0}$ (Lemma 3). Therefore we conclude that the condition $\mathbf{M}_k \mathbf{H}_k = \mathbf{I}_3$ is necessary and sufficient for $E(\hat{\omega}_{k-1}) = \omega_{k-1}$. In this case, the unknown input estimate becomes $\hat{\omega}_{k-1} = \omega_{k-1} + \mathbf{M}_k \mathbf{e}_k$. Finally, the unknown input estimation error is given by $\omega_{k-1} - \hat{\omega}_{k-1} = -\mathbf{M}_k \mathbf{e}_k$.

**Theorem 2.** Assume $\mathbf{R}_{k-1}$ is unbiased, the matrix $\mathbf{H}_k$ is full column rank matrix, and $\mathbf{M}_k = (\mathbf{H}_k^T \tilde{\mathbf{R}}_k^{-1} \mathbf{H}_k)^{-1} \mathbf{H}_k^T \tilde{\mathbf{R}}_k^{-1}$, then the first-order approximation-based estimator (11) is unbiased minimum variance.

*Proof.* Applying Lemma 3 enables us to use the approximated model (17) where $\mathbf{e}_k$ has zero mean and positive definite covariance matrix. Given that the matrix $\mathbf{H}_k$ has full column rank, we satisfy the sufficient conditions for applying the Gauss-Markov Theorem 1. Consequently, the first-order approximation-based estimator (11) is an unbiased minimum variance when $\mathbf{M}_k = (\mathbf{H}_k^T \tilde{\mathbf{R}}_k^{-1} \mathbf{H}_k)^{-1} \mathbf{H}_k^T \tilde{\mathbf{R}}_k^{-1}$.

## 5.3 Prior state estimation

In the following, Lemma 5 finds the prior state estimation error equation to be utilized in the next subsection.

**Lemma 5.** Let $\mathbf{R}_{k-1}$ be unbiased, and the estimator (11) be unbiased, then the first-order approximation-based prior estimator (12) is unbiased, and

$$ \xi_k^* = -\mathbf{M}_k \mathbf{v}_k. \qquad (21) $$

*Proof.* Starting with the prior estimation error equation $\exp_m(\xi_k^*) = \hat{\mathbf{R}}_k^{*-1} \mathbf{R}_k$, then employing the filter's third step (12) yields

$$ \begin{aligned} \exp_m(\xi_k^*) &= \exp_m(-\hat{\omega}_{k-1}) \hat{\mathbf{R}}_{k-1}^{-1} \mathbf{R}_k \\ &= \exp_m(-\hat{\omega}_{k-1}) \exp_m(\xi_{k-1}). \end{aligned} $$

Applying the BCH approximation (7) gives $\xi_k^* = -\hat{\omega}_{k-1} + \xi_{k-1}$, and then substituting (14) gives:

$$ \xi_k^* = \omega_{k-1} - \hat{\omega}_{k-1} + \xi_{k-1}. \qquad (22) $$

Lemma 4 holds, thus (19) and (20) are satisfied. Substituting (20), (15), and (19) sequentially in (22) gives:

$$ \begin{aligned} \xi_k^* &= -\mathbf{M}_k \mathbf{e}_k + \xi_{k-1}, \\ &= -\mathbf{M}_k (\mathbf{H}_k \xi_{k-1} + \mathbf{v}_k) + \xi_{k-1}, \\ &= -\mathbf{M}_k \mathbf{v}_k, \end{aligned} $$

and yields $E(\xi_k^*) = \mathbf{0}$, since $E(\mathbf{v}_{k-1}) = \mathbf{0}$ (unbiased measurement noise).

## 5.4 State estimation

This subsection addresses the estimator (13), where Theorem 3 gives the sufficient conditions of its local optimality, using Lemma 6. We define $\mathbf{H}_k^*$ which is the Jacobian of the observation function with respect to the prior estimation error $\mathbf{H}_k^* = \frac{\partial h(\hat{\mathbf{R}}_k^* \exp_m(\xi))}{\partial \xi} |_{\xi=0}$.

**Lemma 6.** Let $\mathbf{R}_{k-1}$ be unbiased, and the estimator (11) be unbiased, then the first-order approximation-based estimator (13) is unbiased, and

$$ \xi_k = -\mathbf{M}_k \mathbf{v}_k + \mathbf{K}_k \mathbf{H}_k^* \mathbf{M}_k \mathbf{v}_k - \mathbf{K}_k \mathbf{v}_k. \qquad (23) $$
Proof. Starting with the estimation error equation $\exp_m(\xi_k) = \hat{\mathbf{R}}_k^{-1}\mathbf{R}_k$, then employing the filter's fourth step (13) yields

$$ \begin{aligned} \exp_m(\xi_k) &= \exp_m(-\mathbf{K}_k(\mathbf{y}_k - h(\hat{\mathbf{R}}_k^*))\hat{\mathbf{R}}_k^{*-1}\mathbf{R}_k \\ &= \exp_m(-\mathbf{K}_k(\mathbf{y}_k - h(\hat{\mathbf{R}}_k^*))\exp_m(\xi_k^*)). \end{aligned} $$

Applying the BCH approximation (7) gives $\xi_k = \xi_k^* - \mathbf{K}_k(\mathbf{y}_k - h(\hat{\mathbf{R}}_k^*))$, and then substituting (9) gives:

$$ \begin{aligned} \xi_k &= \xi_k^* - \mathbf{K}_k(h(\mathbf{R}_k) - h(\hat{\mathbf{R}}_k^*) + \mathbf{v}_k), \\ &= \xi_k^* - \mathbf{K}_k(h(\hat{\mathbf{R}}_k^* \exp_m(\xi_k^*)) - h(\hat{\mathbf{R}}_k^*) + \mathbf{v}_k), \\ &= \xi_k^* - \mathbf{K}_k \mathbf{H}_k^* \xi_k^* - \mathbf{K}_k \mathbf{v}_k. \end{aligned} \quad (24) $$

Lemma 5 holds, thus (21) is satisfied, substituting it in (24) gives:

$$ \xi_k = -\mathbf{M}_k \mathbf{v}_k + \mathbf{K}_k \mathbf{H}_k^* \mathbf{M}_k \mathbf{v}_k - \mathbf{K}_k \mathbf{v}_k. $$

and yields $E(\xi_k) = 0$, since $E(\mathbf{v}_k) = 0$ (zero mean measurement noise).

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

We aim to minimize $E(\|\xi_k\|^2)$ which is equivalent to minimize the trace of the $\mathbf{P}_k = E(\xi_k \xi_k^T)$. Given that $\xi_{k-1}$ is unbiased, and the estimator (11) is unbiased, then we can utilize (23) (Lemma 6), thus the state estimation error covariance matrix $\mathbf{P}_k = E(\xi_k \xi_k^T)$ has the following expression:

$$ \begin{aligned} \mathbf{P}_k &= E((-\mathbf{M}_k \mathbf{v}_k + \mathbf{K}_k \mathbf{H}_k^* \mathbf{M}_k \mathbf{v}_k - \mathbf{K}_k \mathbf{v}_k)(-\mathbf{M}_k \mathbf{v}_k + \mathbf{K}_k \mathbf{H}_k^* \mathbf{M}_k \mathbf{v}_k - \mathbf{K}_k \mathbf{v}_k)^T), \\ &= (-\mathbf{M}_k - \mathbf{K}_k(\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k))E(\mathbf{v}_k \mathbf{v}_k^T)(-\mathbf{M}_k - \mathbf{K}_k(\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k))^T, \\ &= (\mathbf{M}_k + \mathbf{K}_k(\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k))\mathcal{R}_k(\mathbf{M}_k + \mathbf{K}_k(\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k))^T, \\ &= \mathbf{K}_k ((\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k)\mathcal{R}_k(\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k))^T \mathbf{K}_k^T + \mathbf{M}_k \mathcal{R}_k (\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k)^T \mathbf{K}_k^T + \mathbf{K}_k(\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k)\mathcal{R}_k \mathbf{M}_k^T + \mathbf{M}_k \mathcal{R}_k \mathbf{M}_k^T. \end{aligned} \quad (25) $$

We define two variables $\mathcal{R}_k^*$ and $\mathcal{S}_k^*$ as follows:

$$ \mathcal{R}_k^* = (\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k)\mathcal{R}_k(\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k)^T, \quad (26) $$

$$ \mathcal{S}_k^* = \mathbf{M}_k \mathcal{R}_k (\mathbf{I}_m - \mathbf{H}_k^* \mathbf{M}_k)^T. \quad (27) $$

Thus:

$$ \mathbf{P}_k = \mathbf{K}_k \mathcal{R}^* \mathbf{K}_k^T + \mathcal{S}_k^* \mathbf{K}_k^T + \mathbf{K}_k \mathcal{S}_k^{*T} + \mathbf{M}_k \mathcal{R}_k \mathbf{M}_k^T. \quad (28) $$

By utilizing the matrix derivatives [6, Propositions 10.7.2 and 10.7.4], we obtain:

$$ \frac{d\mathrm{tr}(\mathbf{P}_k)}{d\mathbf{K}_k} = 2\mathbf{K}_k \mathcal{R}_k^* + 2\mathcal{S}_k^*. $$

Minimizing the trace $\mathrm{tr}(\mathbf{P}_k)$ means $\frac{d\mathrm{tr}(\mathbf{P}_k)}{d\mathbf{K}_k} = 0$, thus we aim to find $\mathbf{K}_k$ that satisfies:

$$ \mathbf{K}_k \mathcal{R}_k^* + \mathcal{S}_k^* = 0. \quad (29) $$

The matrix $\mathcal{R}_k^* \in \mathbb{R}^{m \times m}$, which is a real symmetric matrix, has no guarantee to be invertible. Let $p_k$ be the rank of $\mathcal{R}_k^*$, i.e $p_k \le m$. Consequently, there is no unique $\mathbf{K}_k$ that satisfies (29), we then propose $\mathbf{K}_k$ to be in the following form:

$$ \mathbf{K}_k = \mathbf{K}_k^* \mathbf{\Gamma}_k, \quad (30) $$

where $\mathbf{\Gamma}_k \in \mathbb{R}^{p_k \times m}$ is a matrix that satisfies the invertibility of the matrix $\mathbf{\Gamma}_k \mathcal{R}_k^* \mathbf{\Gamma}_k^T \in \mathbb{R}^{p_k \times p_k}$, and $\mathbf{K}_k^* \in \mathbb{R}^{3 \times p_k}$ is designed to minimize the trace $\mathrm{tr}(\mathbf{P}_k)$. Lemma 7 proves that such $\mathbf{\Gamma}_k$ exists, and one solution (the one used in Step 9 in Algorithm 1) is provided in the proof.

**Lemma 7.** For a real symmetric matrix $\mathcal{R}_k^* \in \mathbb{R}^{m \times m}$ with rank $1 \le p_k \le m$, there exists a matrix $\mathbf{\Gamma}_k \in \mathbb{R}^{p_k \times m}$ that satisfies the invertibility of $\mathbf{\Gamma}_k \mathcal{R}_k^* \mathbf{\Gamma}_k^T$.
Proof. The matrix $\mathcal{R}_k^* \in \mathbb{R}^{m \times m}$ is a real symmetric matrix. Thus it can be factored as $\mathcal{R}_k^* = \Gamma_k^* \Lambda_k \Gamma_k^{*T}$, where the columns of $\Gamma_k^* = [\gamma_{k1} \gamma_{k2} \dots \gamma_{kp_k} \dots \gamma_{km}]$ are an orthonormal set of $m$ eigenvectors, the first $p_k$ columns corresponding to the $p_k$ nonzero eigenvalues $(\lambda_{ki})_{1 \le i \le p_k}$, and $\Lambda_k = \text{diag}(\lambda_{k1}, \lambda_{k2}, \dots, \lambda_{kp_k})$, then $\Gamma_k \mathcal{R}_k^* \Gamma_k^T = \text{diag}(\lambda_{k1}, \lambda_{k2}, \dots, \lambda_{kp_k})$, which is invertible. ■

**Theorem 3.** Assume $\mathbf{R}_{k-1}$ is unbiased, the estimator (11) is unbiased, and the gain matrix $\mathbf{K}_k$ is

$$ \mathbf{K}_k = -\mathcal{S}_k^* \Gamma_k^T (\Gamma_k \mathcal{R}_k^* \Gamma_k^T)^{-1} \Gamma_k, \quad (31) $$

then the first-order approximation-based estimator (13) is unbiased minimum variance, and the estimation error covariance matrix is written as follows:

$$ \mathbf{P}_k = \mathbf{K}_k \mathcal{S}_k^{*T} + \mathbf{M}_k \mathcal{R}_k \mathbf{M}_k^T. \quad (32) $$

Proof. Applying Lemma 6 proves the unbiasedness, then we rewrite the state estimation error covariance matrix (28) after substituting (30):

$$ \mathbf{P}_k = \mathbf{K}_k^* \Gamma_k \mathcal{R}_k^* \Gamma_k^T \mathbf{K}_k^{*T} + \mathcal{S}_k^* \Gamma_k^T \mathbf{K}_k^{*T} + \mathbf{K}_k^* \Gamma_k \mathcal{S}_k^{*T} + \mathbf{M}_k \mathcal{R}_k \mathbf{M}_k^T, \quad (33) $$

using matrix derivatives:

$$ \frac{d\text{tr}(\mathbf{P}_k)}{d\mathbf{K}_k^*} = 2\mathbf{K}_k^* \Gamma_k \mathcal{R}_k^* \Gamma_k^T + 2\mathcal{S}_k^* \Gamma_k^T, \quad (34) $$

the value $\mathbf{K}_k^* = -\mathcal{S}_k^* \Gamma_k^T (\Gamma_k \mathcal{R}_k^* \Gamma_k^T)^{-1}$ satisfies

$$ \frac{d\text{tr}(\mathbf{P}_k)}{d\mathbf{K}_k^*} = 2\mathbf{K}_k^* \Gamma_k \mathcal{R}_k^* \Gamma_k^T + 2\mathcal{S}_k^* \Gamma_k^T = \mathbf{0}. $$

Thus the gain $\mathbf{K}_k = \mathbf{K}_k^* \Gamma_k = -\mathcal{S}_k^* \Gamma_k^T (\Gamma_k \mathcal{R}_k^* \Gamma_k^T)^{-1} \Gamma_k$ minimizes $E(\|\xi_k\|^2)$. Additionally, by substituting the value of the optimal $\mathbf{K}_k$ in (33) we can simplify the covariance equation as $\mathbf{P}_k = \mathbf{K}_k \mathcal{S}_k^{*T} + \mathbf{M}_k \mathcal{R}_k \mathbf{M}_k^T$. ■

## 5.5 | Stability analysis

Algorithm 1 is designed to achieve first-order local optimality in the sense of minimizing the variance for the unbiased estimate. However, analyzing its stability remains crucial. In related works, local stability has been studied extensively. For instance, the local stability of the IEKF has been proven for the deterministic case (with the noise turned off) [2]. In [3], the local stability of the IEKF is analyzed in the presence of noise for three engineering examples, including attitude estimation on SO(3) using gyroscope, accelerometer, and magnetometer measurements. In this section, we prove the stability of both the deterministic case and the case with noise.

The estimation error covariance matrix $\mathbf{P}_k$ for any gain matrix $\mathbf{K}_k$ is expressed in (25). Algorithm 1 designs $\mathbf{K}_k$ to minimize the trace of $\mathbf{P}_k$, as shown in Theorem 3. Consequently, the trace of $\mathbf{P}_k$ with $\mathbf{K}_k$ given in 31 is locally minimal among all possible values of $\mathbf{K}_k$, including $\mathbf{0}$. For $\mathbf{K}_k = \mathbf{0}$, from (25), we have:

$$ \mathbf{P}_k \le \mathbf{M}_k \mathcal{R}_k \mathbf{M}_k^T. $$

where $\mathbf{M}_k$ is a pseudo-inverse of $\mathbf{H}_k$, see Theorem 2. The pseudo-inverse $\mathbf{M}_k$ exists and is bounded if and only if $\mathbf{H}_k$ is full column rank [20, Chapter 3.4.2]. As $\mathbf{M}_k$ is bounded, and $\mathcal{R}_k$ represents the covariance of measurement noise, we conclude that $\mathbf{P}_k$ is bounded. This fully establishes that the estimation error covariance matrix remains bounded in the presence of noise. Note that, in the deterministic case where $\mathcal{R}_k = \mathbf{0}$ (i.e., noise is absent), $\mathbf{P}_k$ becomes zero. This result is not surprising, because, with $\mathbf{H}_k$ full column rank, the state can be determined from single time measurements. However, the purpose of the proposed algorithm is to minimize the estimation error in the presence of noise by exploiting both the previous estimate with its covariance matrix and the dynamic model.
6 | APPLICATION TO GYRO-FREE ATTITUDE ESTIMATION

This section presents one important application of the proposed UMV-SO(3) which is the gyro-free attitude estimation, relying only on measurements from two 3-axis sensors: Earth's magnetic field measured by magnetometer, and gravity measured by accelerometer. We compare it with existing solutions in the literature. To better focus on the main contributions of the paper and for sake of simplicity, we considered the case where magnetic disturbances and external accelerations are absent.

The attitude dynamic of rigid body is given by the model (8)-(9), we remind here that the unknown input is $\omega_k = \Delta T \omega_k^{true}$ and $\omega_k^{true} \in \mathbb{R}^3$ is the unknown true angular velocity and $\Delta T$ is the sampling time. The output is $\mathbf{y}_k = (\mathbf{a}_k^b) \in \mathbb{R}^6$, where $\mathbf{a}_k^b \in \mathbb{R}^3$ and $\mathbf{b}_k^b \in \mathbb{R}^3$ are the measured acceleration in the absence of external acceleration, and the magnetic field in the absence of magnetic disturbance, in the body frame, respectively. The output function $h: SO(3) \to \mathbb{R}^6$ is given by $h(\mathbf{R}) = (\frac{\mathbf{R}^{-1}\mathbf{g}}{\mathbf{R}^{-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 written in the Earth's fixed frame, respectively. In order to apply the Algorithm 1, we need to find the first order Taylor expansion (required in steps 2 and 7) and check if the matrix $\mathbf{H}_k$ is full column rank as required. For a general element $\mathbf{R} \in SO(3)$, and a general error $\xi \in \mathbb{R}^3$, we compute:

$$h(\mathbf{R} \exp(\xi)) - h(\mathbf{R}) = \begin{pmatrix} \exp(-\xi)\mathbf{R}^{-1}\mathbf{g} \\ \exp(-\xi)\mathbf{R}^{-1}\mathbf{m}_e \end{pmatrix} - \begin{pmatrix} \mathbf{R}^{-1}\mathbf{g} \\ \mathbf{R}^{-1}\mathbf{m}_e \end{pmatrix}, \\ = \begin{pmatrix} (\exp(-\xi) - \mathbf{I}_3)\mathbf{R}^{-1}\mathbf{g} \\ (\exp(-\xi) - \mathbf{I}_3)\mathbf{R}^{-1}\mathbf{m}_e \end{pmatrix}.$$

Applying the first order approximation (6), and the property (5), consequently give

$$h(\mathbf{R} \exp(\xi)) - h(\mathbf{R}) = \begin{pmatrix} -(\xi) \times \mathbf{R}^{-1}\mathbf{g} \\ -(\xi) \times \mathbf{R}^{-1}\mathbf{m}_e \end{pmatrix}, \\ = \begin{pmatrix} (\mathbf{R}^{-1}\mathbf{g}) \times \\ (\mathbf{R}^{-1}\mathbf{m}_e) \times \end{pmatrix} \xi.$$

Thus the two matrices required in steps 2 and 7 in Algorithm 1 have the following formulas:

$$\mathbf{H}_k = \begin{pmatrix} (\hat{\mathbf{R}}_{klk-1}^{-1}\mathbf{g}) \\ (\hat{\mathbf{R}}_{klk-1}^{-1}\mathbf{m}_e) \end{pmatrix}_{\times}, \quad \mathbf{H}_k^* = \begin{pmatrix} (\hat{\mathbf{R}}_{k}^{*-1}\mathbf{g}) \\ (\hat{\mathbf{R}}_{k}^{*-1}\mathbf{m}_e) \end{pmatrix}_{\times}.$$

The two vectors $\mathbf{g}$ and $\mathbf{m}_e$ are linearly independent, thus according to Lemma 1, both $\mathbf{H}_k$ and $\mathbf{H}_k^*$ are full column rank.

To evaluate the proposed UMV-SO(3), we implemented three algorithms: UMV-SO(3), RTSKF-SO(3) [27], and TRIAD [31]. In order to obtain reliable results, we performed Monte Carlo simulations with 100 runs, and using real datasets.

In both simulations and real experiments, Algorithm 1 is applied recursively. At each time step, the algorithm receives as input the current accelerometer and magnetometer measurements, as well as the attitude estimate and its corresponding estimation error covariance matrix from the previous time step. It then provides the updated attitude estimate and its estimation error covariance matrix, which are used in the subsequent iteration. The initial attitude estimate (at time step zero) can be arbitrarily chosen, and the initial estimation error covariance matrix is set to the identity matrix. As shown in the simulation and experimental results, the algorithm converges regardless of the initial condition.

The comparison is conducted with RTSKF-SO(3) [27], as it is the only dynamic algorithm for attitude estimation that uses measurements solely from a 3-axis accelerometer and a 3-axis magnetometer, without relying on angular velocity measurements. RTSKF-SO(3) has demonstrated superior performance compared to the static algorithm TRIAD [31], as presented in [27]. In this work, we present results for the three algorithms, by conducting Monte Carlo simulations, and by using real datasets, ensuring the reliability of our findings.
## 6.1 Evaluation with simulation

### 6.1.1 Setup of the simulation

The simulated data were generated for a period of 100 sec with a sampling time of $\Delta T = 0.01$ sec. The Earth's gravity vector and the Earth's magnetic field were approximated by these two vectors written in NED (North-East-Down) frame in France: $\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. The angular velocities ground truths were set as shown in Table 1. The standard deviations for the accelerometer and magnetometer noises are assumed to be constants and set to be

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

$\sigma_a = 0.01 \text{ m/s}^2$ and $\sigma_m = 0.005 \text{ Gauss}$, respectively. The initial true rotation matrix was set to correspond to [45°, 45°, 45°] in terms of Euler angles (Roll, Pitch, Yaw) using XYZ convention (Section 3.3 in [13]), while the initial estimation error covariance matrix is set to be identities. When showing the results, and in order to facilitate the interpretation of the results, the rotation matrices were converted into Euler angles using the XYZ convention and expressed in degrees, which is typical in the literature [26].

For each Monte Carlo run, the initial estimate is generated randomly by setting it to correspond to Euler angles, where the three components are uniform random variables taking values between -180 and 180 degrees for roll and yaw, and between -90 and 90 degrees for pitch. These bounds for the uniform random variables cover the range of all possible rotations [13]. Additionally, the measurement noise realizations are generated independently for each run, meaning that each simulation has its own unique set of noise values, distinct from those of other runs, with common noise variances.

### 6.1.2 Simulation results and discussions

Simulation results and discussions are presented in two parts. The first part displays the outcomes obtained from the UMV-SO(3) algorithm, while the second part provides comparisons with RTSKF-SO(3) and TRIAD.

**UMV-SO(3) estimation results:** Fig. 1 plots the estimation error accuracy obtained with UMV-SO(3) for a single Monte Carlo run, where the initial estimate is set to the identity, corresponding to Euler angles with zero components. 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, UMV-SO(3) achieves convergence quickly, specifically in less than 0.05 sec. In the right part, we observe that most of the time the roll error stays between -0.3° and 0.3°, the pitch error remains between -0.12° and 0.12°, and the yaw error is between -2° and 2°. The yaw angle error is notably larger due to its sole reliance on magnetometer measurements. This is because gravity, which has a strictly vertical direction, has no role in determining the yaw angle.

Fig. 2 shows the convergence of the estimation error for 20 runs, covering a wide range of initial estimates. These results demonstrate that convergence is achieved quickly across different initial estimates.

**Comparison of UMV-SO(3) with RTSKF-SO(3) and TRIAD:** Table 2 shows the RMSE for UMV-SO(3), RTSKF-SO(3) [27], and TRIAD [31]. The proposed UMV-SO(3) shows a 36% improvement in RMSE compared to TRIAD, and 19% im-

<table><caption>TABLE 2 RMSE (in degrees) for UMV-SO(3), RTSKF-SO(3), and TRIAD using Monte Carlo simulations</caption><thead><tr><th>UMV-SO(3)</th><th>RTSKF-SO(3)</th><th>TRIAD</th></tr><tr><th>RMSE (degrees)</th><th>RMSE (degrees)</th><th>RMSE (degrees)</th></tr></thead><tbody><tr><td>0.47</td><td>0.58</td><td>0.73</td></tr></tbody></table>
<figure><img src="image_5.png" /><figcaption>FIGURE 1 Attitude estimation error for UMV-SO(3)</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>FIGURE 2 Attitude estimation error for UMV-SO(3), for 20 Monte Carlo runs.</figcaption></figure>

provement compared to the recent RTSKF-SO(3).

The primary objective of our proposed algorithm is attitude estimation, not angular velocity estimation. However, attitude estimation enables the computation of angular velocity by exploiting the rotational dynamic model (the kinematic model of attitude). The estimated attitude is used to derive the angular velocity, followed by a low-pass filter to remove undesired estimation noise, assuming that the angular velocity does not contain high-frequency components. Fig. 3 presents the ground
truth and the estimated angular velocity. To quantify the effectiveness of angular velocity estimation, we calculate the ratio RMSE / RMS, where RMSE is the root mean square error of the angular velocity estimation, and RMS is the root mean square of the true angular velocity. This results in a value of 0.06, indicating a relative estimation error of 6%. A 6% error can be considered acceptable in many applications; however, its acceptability still depends on the specific requirements of the application.

<figure><img src="image_7.png" /><figcaption>FIGURE 3 Angular velocity estimation.</figcaption></figure>

## 6.2 Evaluation with real data

To validate the effectiveness of our proposed algorithm, we test it on real-world datasets obtained from previously conducted experiments. We use the dataset [9], which includes measurements from three commercial MIMU systems (Xsens-MTx, APDM-Opal, and Shimmer3) under different motion conditions (slow, medium, and fast rotation rates). Ground truth orientation is provided by a multi-camera stereo-photogrammetric (SP) system, ensuring high-accuracy reference data. This dataset is available on GitHub (accessible through [9]) and includes well-documented resources that facilitate its usage, along with experiment videos. In our comparison, we use the Xsens-MTx package with medium rotation rates. As shown in the experiment videos and confirmed by sensor measurement plots, the experiments consist of rest phases and moving phases. To ensure a fair comparison, we present results for a window where the system is in motion. Table 3 reports the RMSE for UMV-SO(3), RTSKF-SO(3) [27], and TRIAD [31] over the time window [112, 130] seconds, corresponding to a moving phase. Additionally, Fig. 4 presents the ground truth and UMV-SO(3) attitude estimation for the same time window.

<table><caption>TABLE 3 RMSE (in degrees) for UMV-SO(3), RTSKF-SO(3), and TRIAD using real dataset [9]</caption><thead><tr><th>UMV-SO(3)</th><th>RTSKF-SO(3)</th><th>TRIAD</th></tr><tr><th>RMSE (degrees)</th><th>RMSE (degrees)</th><th>RMSE (degrees)</th></tr></thead><tbody><tr><td>9.11</td><td>9.43</td><td>11.49</td></tr></tbody></table>

The proposed UMV-SO(3) still outperformed TRIAD with a 20% improvement, but its improvement over RTSKF-SO(3) (which we previously proposed in our preliminary conference paper) was reduced to 3.4%. The difference in performance
<figure><img src="image_8.png" /><figcaption>FIGURE 4 Experimental results.</figcaption></figure>

between simulation and real-world data can be attributed to several factors: 1) Assumption Violations in Real Data: The UMV-SO(3) algorithm relies on certain assumptions, such as uncorrelated measurement noises, precise knowledge of covariance matrices, and minimal sensor biases. These assumptions are often met in simulations but may not hold in real-world scenarios, leading to suboptimal performance. 2) RTSKF-SO(3) Robustness: RTSKF-SO(3) is robust algorithm. This makes it less sensitive to unmet conditions. In contrast, UMV-SO(3) is designed for local optimality and is more sensitive to these factors. When real-world assumptions are not fully satisfied, the performance gap narrows.

## 7 | CONCLUSION

This paper presented a state estimation algorithm UMV-SO(3) for systems lying on the special orthogonal groups SO(3), in the presence of unknown input. The first-order approximation-based estimator is proved to be unbiased minimum variance. An important application of the proposed UMV-SO(3) is applied in gyro-free attitude estimation. Monte Carlo simulations validated the proposed algorithm, demonstrating its superior performance in attitude estimation using measurements solely from a 3-axis accelerometer and a 3-axis magnetometer and under unknown angular velocity in dynamics. The proposed approach outperforms RTSKF-SO(3) and TRIAD algorithms. In future, we aim to implement the algorithm with alternative output functions, such as using camera images. Additionally, future research may explore extending this work to velocity estimation. Finally, generalizing UMV-SO(3) for systems lay on Lie groups is possible, where SO(3) group is a special case of Lie groups.

## DATA AVAILABILITY STATEMENT

The data supporting the findings of this study are openly available in the *mimu_optical_dataset_caruso_sassari* repository at https://github.com/marcocaruso/mimu_optical_dataset_caruso_sassari/releases/tag/v5.0, DOI: 10.1109/JSEN.2020.3024806. For detailed information about the dataset, please refer to Caruso et al. (2021) [9].

## REFERENCES

1. A. Barrau and S. Bonnabel, *Intrinsic filtering on Lie groups with applications to attitude estimation*, IEEE Transactions on Automatic Control **60** (2015), no. 2, 436–449.

2. A. Barrau and S. Bonnabel, *The invariant extended Kalman filter as a stable observer*, IEEE Transactions on Automatic Control **62** (2017), no. 4, 1797–1812.

3. A. Barrau and S. Bonnabel, *Three examples of the stability properties of the invariant extended Kalman filter*, IFAC-PapersOnLine **50** (2017), no. 1, 431–437.

4. S. Berkane, A. Abdessameud, and A. Tayebi, *Hybrid attitude and gyro-bias observer design on SO(3)*, IEEE Transactions on Automatic Control **62** (2017), no. 11, 6044–6050.

5. S. Berkane and A. Tayebi, *On the design of attitude complementary filters on SO(3)*, IEEE Transactions on Automatic Control **63** (2018), no. 3, 880–887.

6. D. Bernstein, *Matrix mathematics: theory, facts, and formulas*, 2nd edn., Princeton University Press, 2009.
7. G. Bourmaud, R. Mégret, M. Arnaudon, and A. Giremus, *Continuous-discrete extended Kalman filter on matrix Lie groups using concentrated gaussian distributions*, Journal of Mathematical Imaging and Vision **51** (2015), no. 1, 209-228.

8. M. Brossard, S. Bonnabel, and A. Barrau, *Unscented Kalman filter on Lie groups for visual inertial odometry*, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE, Madrid, 2018, 649-655.

9. M. Caruso, A. M. Sabatini, M. Knaflitz, M. Gazzoni, U. D. Croce, and A. Cereatti, *Orientation estimation through magneto-inertial sensor fusion: A heuristic approach for suboptimal parameters tuning*, IEEE Sensors Journal **21** (2021), no. 3, 3408-3419.

10. J. L. Crassidis, F. L. Markley, and Y. Cheng, *Survey of nonlinear attitude estimation methods*, Journal of Guidance, Control, and Dynamics **30** (2007), no. 1, 12-28.

11. H. Fang, R. De Callafon, and P. Franks, *Smoothed estimation of unknown inputs and states in dynamic systems with application to oceanic flow field reconstruction*, International Journal of Adaptive Control and Signal Processing **29** (2015), no. 10, 1224-1242.

12. H. Fang, T. Srivas, R. De Callafon, and M. Haile, *Ensemble-based simultaneous input and state estimation for nonlinear dynamic systems with application to wildfire data assimilation*, Control Engineering Practice **63** (2017), 104-115.

13. H. Fourati and D. Belkhiat (eds.) *Multisensor attitude estimation: fundamental concepts and applications*, CRC Press, 2016.

14. S. Gillijns and B. De Moor, *Unbiased minimum-variance input and state estimation for linear discrete-time systems*, Automatica **43** (2007), no. 1, 111-116.

15. S. Gillijns and B. De Moor, *Unbiased minimum-variance input and state estimation for linear discrete-time systems with direct feedthrough*, Automatica **43** (2007), no. 5, 934-937.

16. R. Hamrah, R. R. Warier, and A. K. Sanyal, *Finite-time stable estimator for attitude motion in the presence of bias in angular velocity measurements*, Automatica **132** (2021), 109815.

17. T. Hiller, Z. Pentek, J.-T. Liewald, A. Buhmann, and H. Roth, *Origins and mechanisms of bias instability noise in a three-axis mode-matched MEMS gyroscope*, Journal of Microelectromechanical Systems **28** (2019), no. 4, 586-596.

18. C.-S. Hsieh, *Robust two-stage Kalman filters for systems with unknown inputs*, IEEE Transactions on Automatic Control **45** (2000), no. 12, 2374-2378.

19. C.-S. Hsieh, *State estimation for nonlinear systems with unknown inputs*, IEEE 7th Conference on Industrial Electronics and Applications (ICIEA), Singapore, Singapore, 2012, 1533-1538.

20. T. Kailath, A. Sayed, and B. Hassibi, *Linear estimation*, Prentice Hall, 2000.

21. A. Khosravian, J. Trumpf, R. Mahony, and C. Lageman, *Observers for invariant systems on Lie groups with biased input measurements and homogeneous outputs*, Automatica **55** (2015), 19-26.

22. C. Liu, G. Vukovich, K. Shi, and Z. Sun, *Robust fault tolerant nonfragile $H_\infty$ attitude control for spacecraft via stochastically intermediate observer*, Advances in Space Research **62** (2018), no. 9, 2631-2648.

23. B. Lyu, C. Liu, and X. Yue, *Hybrid nonfragile intermediate observer-based T-S fuzzy attitude control for flexible spacecraft with input saturation*, Aerospace Science and Technology **128** (2022), 107753.

24. A. Makni, H. Fourati, and A. Kibangou, *Energy-aware adaptive attitude estimation under external acceleration for pedestrian navigation*, IEEE/ASME Transactions on Mechatronics **21** (2016), no. 3, 1366-1375.

25. 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 Systems Letters **3** (2019), no. 2, 392-397.

26. A. M. Sabatini, *Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing*, IEEE Transactions on Biomedical Engineering **53** (2006), no. 7, 11.

27. G. Shaaban, H. Fourati, A. Kibangou, and C. Prieur, *Gyro-free Kalman filter with unknown inputs for SO(3)-based attitude estimation*, IEEE 13th International Conference on Indoor Positioning and Indoor Navigation (IPIN), Nuremberg, Germany, 2023, 1-6.

28. G. Shaaban, H. Fourati, A. Kibangou, and C. Prieur, *MARG sensor-based attitude estimation on SO(3) under unknown external acceleration*, IEEE Control Systems Letters **7** (2023), 3795-3800.

29. G. Shaaban, H. Fourati, A. Kibangou, and C. Prieur, *Position, velocity, and attitude estimation based on marg and position measurements under unknown external acceleration*, IEEE Control Systems Letters (2025).

30. G. Shaaban, H. Fourati, A. Kibangou, and C. Prieur, *Secure MARG sensor-based attitude estimation on SO(3) under randomly occurring false data injection attacks*, IEEE 23rd European Control Conference (ECC), Thessaloniki, Greece, 2025, 1-6.

31. M. D. Shuster and S. D. Oh, *Three-axis attitude determination from vector observations*, J. Guid. Control Dyn. **4** (1981), no. 1, 70-77.

32. X. Song and W. Zheng, *A Kalman-filtering derivation of input and state estimation for linear discrete-time systems with direct feedthrough*, Automatica **161** (2024), 111453.

33. C.-W. Tan, S. Park, K. Mostov, and P. Varaiya, *Design of gyroscope-free navigation systems*, ITSC 2001. 2001 IEEE Intelligent Transportation Systems. Proceedings (Cat. No.01TH8585), Oakland, CA, USA, 2001, 286-291.

34. Y.-L. Tsai, T.-T. Tu, H. Bae, and P. Chou, *EcoIMU: A dual triaxial-accelerometer inertial measurement unit for wearable applications*, International Conference on Body Sensor Networks (BSN), IEEE, Singapore, Singapore, 2010, 207-212.

35. G. Wahba, *Problem 65-1: A least squares estimate of satellite attitude*, SIAM Review **7** (1965), no. 3, 409-409.

36. N. Wang, R. Hamrah, A. K. Sanyal, and M. N. Glauser, *Geometric extended state observer on TSE(3) with fast finite-time stability: Theory and validation on a multi-rotor vehicle*, Aerospace Science and Technology **155** (2024), 109596.

37. L. Yin, W. Xie, S. Wang, and V. Sreeram, *Simultaneous input and state estimation: From a unified least-squares perspective*, Automatica **171** (2025), 111906.

38. S. Yong, M. Zhu, and E. Frazzoli, *A unified filter for simultaneous input and state estimation of linear discrete-time stochastic systems*, Automatica **63** (2016), 321-329.

39. X. Yun, E. R. Bachmann, and R. B. McGhee, *A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements*, IEEE Transactions on Instrumentation and Measurement **57** (2008), no. 3, 638-650.