# Position, Velocity, and Attitude Estimation Based on MARG and Position Measurements Under Unknown External Acceleration

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, Christophe Prieur

► To cite this version:

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, Christophe Prieur. Position, Velocity, and Attitude Estimation Based on MARG and Position Measurements Under Unknown External Acceleration. IEEE Control Systems Letters, 2025, 9, pp.1423-1428. 10.1109/LCSYS.2025.3579402. hal-05113993

HAL Id: hal-05113993

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

Submitted on 16 Jun 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
# Position, Velocity, and Attitude Estimation Based on MARG and Position Measurements Under Unknown External Acceleration

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

**Abstract**— Estimation of position, velocity, and attitude of a rigid body is crucial for various applications. Many of these rely on position and Magnetic, Angular Rate, and Gravity (MARG) sensors, which consist of a gyroscope measuring angular velocity, a magnetometer measuring the magnetic field, and an accelerometer measuring gravity along with an unknown external acceleration. This external acceleration also appears in the kinematic equation of velocity. In this paper, a position, velocity, and SO(3) attitude estimation algorithm is designed differently by considering the problem of external acceleration as an unknown input affecting both the dynamic model and measurements. The effectiveness of the algorithm is validated through Monte Carlo simulations.

**Index Terms**— PVA estimation, External acceleration, Estimation under unknown input, SO(3), Position sensors.

## I. INTRODUCTION

ESTIMATION of the position, velocity, and attitude (PVA) of a rigid body is crucial for various applications, including robotics, autonomous navigation, and aerospace systems. Position can be directly measured using the Global Positioning System (GPS) [6]. Attitude, unlike position, cannot be directly measured by a single sensor and is instead estimated using fusion techniques that combine measurements from multiple sensors [3]. A common combination includes the magnetic field measured by a magnetometer, angular velocity measured by a gyroscope, and acceleration measured by an accelerometer, thus forming the Magnetic, Angular Rate, and Gravity (MARG) sensors. Modern navigation systems commonly integrate MARG and position sensors to achieve PVA estimation [17], due to their low cost, ease of installation, and suitability for a wide range of applications.

The position and velocity model, with acceleration in the fixed navigation frame as input and position measurements as output, forms a linear state-space system where both velocity and position are observable. However, since acceleration is measured in the body frame, its transformation to the navigation frame requires knowledge of the rotation between the two frames. A common approach in the literature is the

cascaded method [19], where attitude is estimated first and then used for position and velocity estimation. One classical approach to estimating attitude relies on static algorithms that utilize two known noncolinear reference vectors in both body and navigation frames. This problem is formally defined as Wahba's problem [4]. These reference vectors are typically Earth's gravity and magnetic field, which are known in the fixed navigation frame and measured in the body frame by an accelerometer and magnetometer, respectively, assuming no external acceleration or magnetic disturbances. A well-known solution to Wahba's problem is the TRIAD algorithm [14]. To improve the accuracy of attitude estimation, several algorithms incorporate angular velocity measurements in addition to acceleration and magnetic field. These methods include complementary filters [9] and Kalman-like filters, such as the widely used extended Kalman filter (EKF), where attitude is represented using quaternions [11], [15]. However, quaternion-based EKFs require normalization at each update step, which can introduce a loss in accuracy. An improved algorithm represents attitude as an element of the special orthogonal group SO(3) [1].

A common limitation of the aforementioned attitude estimation algorithms based on MARG sensors is their assumption that the accelerometer measures only gravity in the body frame, neglecting external acceleration [10]. However, this assumption becomes invalid in scenarios involving periods of accelerated motions, when the external acceleration is non-negligible compared to gravitational acceleration. Several works have addressed this problem. A threshold-based adaptive method [11] discards accelerometer measurements when external acceleration is detected, but during extended acceleration phases, it relies solely on magnetometer data, making attitude unobservable. Solutions for PVA estimation exist in the literature, differently from the cascaded approach, deterministic observers without considering sensor noise have been proposed in [2], [7].

In the recent work [13], inspired by unknown input filtering algorithms [5], [12], [18], a locally optimal algorithm for attitude estimation on SO(3) based on MARG sensors was designed by treating external acceleration as an unknown input affecting the output. In the current work, we aim to design a locally optimal algorithm for position, velocity, and attitude estimation based on MARG and position sensors, under unknown external acceleration that affects both the dynamic model and the output, where external acceleration appears in

Ghadeer Shaaban, Hassen Fourati, Alain Kibangou, and Christophe Prieur are with Univ. Grenoble Alpes, CNRS, Inria, Grenoble INP, GIPSA-lab, Grenoble, France. Alain Kibangou is also with the Faculty of Science, University of Johannesburg (Auckland Park Campus), Johannesburg 2006, South Africa (e-mails: ghadeer.shaaban@gipsa-lab.fr; hassen.fourati@gipsa-lab.fr; alain.kibangou@gipsa-lab.fr; christophe.prieur@gipsa-lab.fr)
the velocity dynamic (kinematic) equation. Additionally, the velocity dynamic equation includes a multiplicative coupling between the attitude and external acceleration, introducing new theoretical challenges beyond those in [13].

The main contributions of this paper are: 1) The design of a locally unbiased, minimum-variance state estimator for systems on SO(3)×R³ × R³, with an unknown input affecting both dynamics and output, and multiplicatively coupled with the state on SO(3). 2) The design of a novel estimation algorithm for position, velocity, attitude and external acceleration based on MARG and position sensors.

The remainder of this paper is as follows: Section II provides preliminaries. Section III explains the attitude, position, and velocity dynamic models, as well as the sensor output model and the problem statement. Section IV presents the proposed algorithm, with its steps detailed in Section V. Section VI validates the effectiveness of the algorithm through Monte Carlo simulations. Section VII concludes the paper.

## II. PRELIMINARIES

In this paper, matrices, vectors, and scalars are denoted by bold upper-case, bold lower-case, and regular lower-case symbols, respectively. The notations $O(\cdot)$, $E(\cdot)$, $\hat{z}$, $\mathbf{I}_n$, $\mathbf{0}$, $\mathbf{z}(i: j)$ and $\mathbf{Z}(i: j)$ correspond to the higher-order terms neglected in an approximation, the expectation operator, the trace operator, the estimate of a variable $\mathbf{z}$, the identity matrix of size $n$, the zero matrix, components from $i$ to $j$ of vector $\mathbf{z}$, and lines from $i$ to $j$ of matrix $\mathbf{Z}$, respectively.

Attitude can be represented mathematically by rotation matrices belonging to the special orthogonal group SO(3) = {$\mathbf{R} \in \mathbb{R}^{3 \times 3} : \mathbf{R}\mathbf{R}^T = \mathbf{I}_3, \det(\mathbf{R}) = 1$} [2], [9]. The exponential map $\exp_m : \mathbb{R}^3 \to \text{SO}(3)$ maps an element $\xi \in \mathbb{R}^3$ to an element in SO(3) using the following formula [2], [9]: $\exp_m(\mathbf{0}) = \mathbf{I}_3$ and $\exp_m(\xi) = \mathbf{I}_3 + \frac{\sin(\|\xi\|)}{\|\xi\|}(\xi)_\times + 2\frac{\sin(\|\xi\|/2)}{\|\xi\|}(\xi)_\times, \forall \xi \in \mathbb{R}^3 \setminus \{\mathbf{0}\}$, where $(\xi)_\times = \begin{pmatrix} 0 & -\xi_3 & \xi_2 \\ \xi_3 & 0 & -\xi_1 \\ -\xi_2 & \xi_1 & 0 \end{pmatrix}$ is the skew-symmetric matrix. Letting $\xi_1, \xi_2 \in \mathbb{R}^3$, we have [2], [9]: $\exp_m(\xi_1)^{-1} = \exp_m(-\xi_1)$ and $(\xi_1)_\times \xi_2 = -(\xi_2)_\times \xi_1$. The exponential map is approximated to first order as follows [9]:

$$ \exp_m(\xi_1) = \mathbf{I}_3 + (\xi_1)_\times + \mathcal{O}(\|\xi_1\|^2), \quad (1) $$

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

Equation (2) expresses the Baker-Campbell-Hausdorff (BCH) formula, which is commonly employed in the design of Kalman filters on SO(3).

## III. PROBLEM STATEMENT

Consider the following discrete-time dynamic model [2]:

$$ \mathbf{R}_{k+1} = \mathbf{R}_k \exp_m((\omega_k + \mathbf{w}_k^\omega) dt), \quad (3) $$

$$ \mathbf{v}_{k+1} = \mathbf{v}_k + \mathbf{R}_k \mathbf{a}_k^{\text{ext}} dt, \quad (4) $$

$$ \mathbf{x}_{k+1} = \mathbf{x}_k + \mathbf{v}_k dt, \quad (5) $$

where $\mathbf{R}_k \in \text{SO}(3)$, $\mathbf{v}_k \in \mathbb{R}^3$, and $\mathbf{x}_k \in \mathbb{R}^3$ are the attitude, velocity, and position, respectively. The angular velocity measurement is denoted by $\omega_k$, its noise by $\mathbf{w}_k^\omega$, and sampling

time by $dt$. The unknown external acceleration of the rigid body is denoted by $\mathbf{a}_k^{\text{ext}}$, and it is written in the body frame. Note that the term $\mathbf{R}_k \mathbf{a}_k^{\text{ext}}$ in (4) is the external acceleration of the rigid body written in fixed frame, which is the derivation of velocity in the fixed frame. The discrete output model is [2]:

$$ \mathbf{a}_k = \mathbf{R}_k^T \mathbf{g} + \mathbf{a}_k^{\text{ext}} + \mathbf{w}_k^a, \quad (6) $$

$$ \mathbf{b}_k = \mathbf{R}_k^T \mathbf{m}_e + \mathbf{w}_k^b, \quad (7) $$

$$ \mathbf{p}_k = \mathbf{x}_k + \mathbf{w}_k^p, \quad (8) $$

where $\mathbf{a}_k$, $\mathbf{b}_k$, $\mathbf{p}_k \in \mathbb{R}^3$, are the accelerometer, magnetometer, and position sensor measurements respectively, and their associated measurement noises are $\mathbf{w}_k^a, \mathbf{w}_k^b, \mathbf{w}_k^p \in \mathbb{R}^3$ respectively. The Earth's gravity acceleration and magnetic field written in the Earth's fixed frame are denoted by $\mathbf{g}, \mathbf{m}_e$, respectively. In order to facilitate the writing throughout the paper, we define the function $h(\mathbf{R}, \mathbf{x}) = \begin{pmatrix} \mathbf{R}^T \mathbf{g} \\ \mathbf{R}^T \mathbf{m}_e \end{pmatrix}$, the matrix $\mathbf{D} = \begin{pmatrix} \mathbf{I}_3 \\ \mathbf{0}_3 \\ \mathbf{0}_3 \end{pmatrix}$, and the measurement noise $\mathbf{w}_k^y = \begin{pmatrix} \mathbf{w}_k^a \\ \mathbf{w}_k^b \\ \mathbf{w}_k^p \end{pmatrix}$, thus we can write the output model as follows:

$$ \mathbf{y}_k = h(\mathbf{R}_k, \mathbf{x}_k) + \mathbf{D} \mathbf{a}_k^{\text{ext}} + \mathbf{w}_k^y. \quad (9) $$

The noises $\mathbf{w}_k^a$ and $\mathbf{w}_k^b$ are assumed to be uncorrelated zero-mean white random signals with positive definite covariance matrices $\mathcal{Q}_k$ and $\mathcal{R}_k$ respectively.

The attitude state model which is given by the dynamic (3) and the output (6)-(7) is left invertible (see [16, Def. 2.5]) and observable (see [16, Def. 2.4]), but not strongly observable (see [16, Def. 2.6]). More details exist in [13]. The velocity and position model given by the dynamic (4)-(5) and the output (8) is observable given $\mathbf{a}_k^{\text{ext}}$. Therefore, the unknown external acceleration $\mathbf{a}_k^{\text{ext}}$, attitude $\mathbf{R}_k$, velocity $\mathbf{v}_k$, and position $\mathbf{x}_k$ can be uniquely reconstructed based on sensor measurements and the initial attitude state $\mathbf{R}_0$.

Motivated by the previous paragraph, this paper aims to design a filter that estimates the attitude, velocity, and position of a rigid body in the presence of unknown external acceleration, using MARG and position sensor measurements, along with an initial unbiased estimate of the attitude. To achieve this, we design a novel recursive algorithm PVA-SO(3) that provides an unbiased, minimum variance first-order approximation-based estimator for Position $\mathbf{x}_k$, Velocity $\mathbf{v}_k$, and Attitude $\mathbf{R}_k \in \text{SO}(3)$, using the unbiased estimates of $\mathbf{R}_{k-1}$, $\mathbf{v}_{k-1}$, and $\mathbf{x}_{k-1}$ and the measurements $\omega_k$ and $\mathbf{y}_k$. This recursive algorithm successfully achieves our objective by ensuring that, starting from an unbiased estimate of the attitude, the attitude estimate remains unbiased and the variance is minimized at each step. Additionally, since velocity and position are observable, their estimates converge regardless of the initial values. It is important to note that obtaining an unbiased, slightly perturbed initial estimate of the attitude is feasible using static algorithms, such as the TRIAD method [14], which relies on the initial readings from the magnetometer and accelerometer.

## IV. FILTER DESIGN

In this work, an unknown input filtering algorithm is designed to estimate the external acceleration as an intermediate
step for PVA estimation. We adopt the three-step procedure from our previous work on attitude estimation using MARG measurements [13], which was inspired by the unknown input filtering method proposed in [5], originally developed for linear systems. The three steps are: (i) state prediction based on the dynamic model and previous estimates, (ii) external acceleration estimation based on the state prediction and measurements, and (iii) state estimation correction using the state prediction, external acceleration estimation, and measurements. The current work presents three main challenges beyond our previous work [13]. First, the state includes two additional vectors in $\mathbb{R}^3$, along with the attitude, which belongs to SO(3). Second, in contrast to [13], where the unknown external acceleration affects only the measurements, in the current work, it affects both the dynamic model and the measurements. This creates a coupling between state prediction and external acceleration estimation. Third, the dynamic model involves a multiplication of the rotation matrix with the unknown external acceleration. This makes the derivation of the prediction error transition matrix challenging.

We denote the estimation of $\mathbf{x}_k$, $\mathbf{v}_k$, $\mathbf{R}_k$, and $\mathbf{a}_k^{\text{ext}}$ by $\hat{\mathbf{x}}_k$, $\hat{\mathbf{v}}_k$, $\hat{\mathbf{R}}_k$, and $\hat{\mathbf{a}}_k^{\text{ext}}$, respectively. We denote the state estimation error by $\mathbf{e}_k = \begin{pmatrix} \xi_k \\ \mathbf{e}_k^v \\ \mathbf{e}_k^x \end{pmatrix}$, and the external acceleration estimation error by $\mathbf{e}_k^a = \mathbf{a}_k^{\text{ext}} - \hat{\mathbf{a}}_k^{\text{ext}}$, where $\exp_m(\xi_k) = \hat{\mathbf{R}}_k^{-1}\mathbf{R}_k$ (error on SO(3) definition [9]), $\mathbf{e}_k^v = \mathbf{v}_k - \hat{\mathbf{v}}_k$, and $\mathbf{e}_k^x = \mathbf{x}_k - \hat{\mathbf{x}}_k$ are the attitude, velocity, and position estimation error, respectively. We define the corresponding covariance matrices: $\mathbf{P}_k^e = E(\mathbf{e}_k \mathbf{e}_k^T)$, $\mathbf{P}_k^a = E(\mathbf{e}_k^a \mathbf{e}_k^a T)$, and $\mathbf{P}_k^{\text{ae}} = (\mathbf{P}_k^{\text{ea}})^T = E(\mathbf{e}_k^a \mathbf{e}_k^T)$. Algorithm 1 presents the detailed steps of PVA-SO(3). The input to this algorithm consists of the state and external acceleration estimates, along with their corresponding covariance matrices, and the MARG and position measurements. The output is the state and external acceleration estimates for the current time step. The derivation of algorithm steps is detailed in Section V.

**Main Results:** Assuming that $\hat{\mathbf{R}}_{k-1}$, $\hat{\mathbf{v}}_{k-1}$, $\hat{\mathbf{x}}_{k-1}$, and $\hat{\mathbf{a}}_{k-1}^{\text{ext}}$ are unbiased, we demonstrate that the first-order approximation-based estimators for $\hat{\mathbf{R}}_k$, $\hat{\mathbf{v}}_k$, $\hat{\mathbf{x}}_k$, and $\hat{\mathbf{a}}_k^{\text{ext}}$ provided by Algorithm 1 are unbiased and minimum variance (see Lemma 5.2, Theorem 5.4 and Theorem 5.8).

# V. FILTER ANALYSIS

## A. State prediction

**Lemma 5.1:** Consider two elements, $\mathbf{R} \in \text{SO}(3)$ and $\mathbf{a} \in \mathbb{R}^3$, with their respective estimations $\hat{\mathbf{R}} \in \text{SO}(3)$ and $\hat{\mathbf{a}} \in \mathbb{R}^3$, and their respective estimation errors $\exp_m(\xi) = \hat{\mathbf{R}}^{-1}\mathbf{R}$ and $\mathbf{e}^a = \mathbf{a} - \hat{\mathbf{a}}$. The following equation holds:

$$ \mathbf{R}\mathbf{a} - \hat{\mathbf{R}}\hat{\mathbf{a}} = -\hat{\mathbf{R}}(\hat{\mathbf{a}})_{\times} \xi + \hat{\mathbf{R}}\mathbf{e}^a + \mathcal{O}(\|\xi\|^2, \|\xi\|\cdot\|\mathbf{e}^a\|). $$

*Proof:* $\mathbf{R}\mathbf{a} = \hat{\mathbf{R}} \exp_m(\xi)(\hat{\mathbf{a}} + \mathbf{e}^a) = \hat{\mathbf{R}}(\mathbf{I}_3 + (\xi)_{\times} + \mathcal{O}(\|\xi\|^2))(\hat{\mathbf{a}} + \mathbf{e}^a) = \hat{\mathbf{R}}\hat{\mathbf{a}} + \hat{\mathbf{R}}(\xi)_{\times}\hat{\mathbf{a}} + \hat{\mathbf{R}}\mathbf{e}^a + \mathcal{O}(\|\xi\|^2) + \hat{\mathbf{R}}(\xi)_{\times}\mathbf{e}^a$, thus $\mathbf{R}\mathbf{a} - \hat{\mathbf{R}}\hat{\mathbf{a}} = -\hat{\mathbf{R}}(\hat{\mathbf{a}})_{\times}\xi + \hat{\mathbf{R}}\mathbf{e}^a + \mathcal{O}(\|\xi\|^2, \|\xi\|\cdot\|\mathbf{e}^a\|)$. ■

**Lemma 5.2:** Let $\mathbf{R}_{k-1}$, $\hat{\mathbf{v}}_{k-1}$, $\hat{\mathbf{x}}_{k-1}$, and $\hat{\mathbf{a}}_{k-1}^{\text{ext}}$ be unbiased, then the state predictions in steps 1–3 of Algorithm 1 are unbiased, with step 4 providing the prediction error covariance matrix.

## Algorithm 1 PVA-SO(3)

**Inputs:** $\hat{\mathbf{R}}_{k-1}$, $\hat{\mathbf{v}}_{k-1}$, $\hat{\mathbf{x}}_{k-1}$, $\hat{\mathbf{a}}_{k-1}^{\text{ext}}$, $\mathbf{P}_{k-1}^e$, $\mathbf{P}_{k-1}^a$, $\mathbf{P}_{k-1}^{\text{ae}}$, $\omega_{k-1}$, $y_k = \begin{pmatrix} \mathbf{a}_k^b \\ \mathbf{b}_k^b \\ \mathbf{p}_k^b \end{pmatrix}$.

▷ State prediction:

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

2: $\hat{\mathbf{v}}_{k|k-1} = \hat{\mathbf{v}}_{k-1} + \hat{\mathbf{R}}_{k-1} \hat{\mathbf{a}}_{k-1}^{\text{ext}} dt$

3: $\hat{\mathbf{x}}_{k|k-1} = \hat{\mathbf{x}}_{k-1} + \hat{\mathbf{v}}_{k-1} dt$

4: $\mathbf{A}_{k-1} = \begin{pmatrix} \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ -\hat{\mathbf{R}}_{k-1}(\hat{\mathbf{a}}_{k-1}^{\text{ext}})_{\times} dt & \mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{I}_3 dt & \mathbf{I}_3 \end{pmatrix}$, $\mathbf{E}_{k-1} = \begin{pmatrix} \mathbf{0}_3 \\ \hat{\mathbf{R}}_{k-1} dt \\ \mathbf{0}_3 \end{pmatrix}$, $\mathbf{G} = \begin{pmatrix} \mathbf{I}_3 dt \\ \mathbf{0}_3 \\ \mathbf{0}_3 \end{pmatrix}$

$$ \mathbf{P}_{k|k-1}^e = (\mathbf{A}_{k-1} \quad \mathbf{E}_{k-1}) \begin{pmatrix} \mathbf{P}_{k-1}^e & \mathbf{P}_{k-1}^{\text{ae}} \\ \mathbf{P}_{k-1}^{\text{ae}} & \mathbf{P}_{k-1}^a \end{pmatrix} \begin{pmatrix} \mathbf{A}_{k-1}^T \\ \mathbf{E}_{k-1}^T \end{pmatrix} + \mathbf{G}\mathbf{Q}_{k-1}\mathbf{G}^T $$

▷ External acceleration estimation:

5: $\mathbf{H}_k = \begin{pmatrix} (\hat{\mathbf{R}}_{k|k-1}^T \mathbf{g})_{\times} & \mathbf{0}_3 & \mathbf{0}_3 \\ (\hat{\mathbf{R}}_{k|k-1}^T \mathbf{m}_e)_{\times} & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 \end{pmatrix}$

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

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

8: $\hat{\mathbf{a}}_k^{\text{ext}} = \mathbf{M}_k (y_k - h(\hat{\mathbf{R}}_{k|k-1}, \hat{\mathbf{x}}_{k|k-1}))$

9: $\mathbf{P}_k^a = (\mathbf{D}^T \tilde{\mathbf{R}}_k^{-1} \mathbf{D})^{-1}$

▷ Correction:

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

11: $\delta_k = \mathbf{K}_k (y_k - h(\hat{\mathbf{R}}_{k|k-1}, \mathbf{x}_{k|k-1}) - \mathbf{D}\hat{\mathbf{a}}_k^{\text{ext}})$

12: $\hat{\mathbf{R}}_k = \hat{\mathbf{R}}_{k|k-1} \exp_m(\delta_k(1:3))$

13: $\hat{\mathbf{v}}_k = \hat{\mathbf{v}}_{k|k-1} + \delta_k(4:6)$

14: $\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_{k|k-1} + \delta_k(7:9)$

15: $\mathbf{P}_k^{\text{ae}} = -\mathbf{P}_{k|k-1}^e \mathbf{H}_k^T \mathbf{M}_k^T$

16: $\mathbf{P}_k^e = \mathbf{P}_k^e - \mathbf{K}_k (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \mathbf{H}_k \mathbf{P}_k^e$

17: return $\hat{\mathbf{R}}_k$, $\hat{\mathbf{v}}_k$, $\hat{\mathbf{x}}_k$, $\hat{\mathbf{a}}_k^{\text{ext}}$, $\mathbf{P}_k^e$, $\mathbf{P}_k^a$, $\mathbf{P}_k^{\text{ae}}$

*Proof:* The attitude prediction error is given by: $\exp_m(\xi_{k|k-1}) = \hat{\mathbf{R}}_{k|k-1}^T \mathbf{R}_k = \exp_m(-\omega_{k-1} dt) \hat{\mathbf{R}}_{k|k-1}^T \mathbf{R}_{k-1} \exp_m(\omega_{k-1} dt + \mathbf{w}_{k-1}^\omega dt) = \exp_m(-\omega_{k-1} dt) \exp_m(\xi_{k-1}) \exp_m(\omega_{k-1} dt + \mathbf{w}_{k-1}^\omega dt)$. Applying BCH formula (2) gives

$$ \xi_{k|k-1} = \xi_{k-1} + \mathbf{w}_{k-1}^\omega dt. \quad (10) $$

The velocity prediction error is:

$$ \begin{aligned} \mathbf{e}_{k|k-1}^v &= \mathbf{v}_k - \hat{\mathbf{v}}_{k|k-1}, \\ &= \mathbf{v}_{k-1} + \mathbf{R}_{k-1} \mathbf{a}_{k-1}^{\text{ext}} dt - \hat{\mathbf{v}}_{k-1} - \hat{\mathbf{R}}_{k-1} \hat{\mathbf{a}}_{k-1}^{\text{ext}} dt, \\ &= \mathbf{e}_{k-1}^v + (\mathbf{R}_{k-1} \mathbf{a}_{k-1}^{\text{ext}} - \hat{\mathbf{R}}_{k-1} \hat{\mathbf{a}}_{k-1}^{\text{ext}}) dt. \end{aligned} $$
Applying Lemma 5.1 on the term $\mathbf{R}_{k-1}a_{k-1}^{\text{ext}}$ we obtain:

$$e_{k|k-1}^{v} = e_{k-1}^{v} - \hat{\mathbf{R}}_{k-1}(\hat{\mathbf{a}}_{k-1}^{\text{ext}}) \times \xi_{k-1} dt + \hat{\mathbf{R}}_{k-1} e_{k-1}^{a} dt. \quad (11)$$

The position prediction error is:

$$\begin{aligned} e_{k|k-1}^{x} &= \mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1} = \mathbf{x}_{k-1} + \mathbf{v}_{k-1} dt - \hat{\mathbf{x}}_{k-1} - \hat{\mathbf{v}}_{k-1} dt, \\ &= e_{k-1}^{x} + e_{k-1}^{v} dt. \end{aligned} \quad (12)$$

Concatenating (10), (11), and (12) gives:

$$\begin{aligned} \mathbf{e}_{k|k-1} = & \begin{pmatrix} \mathbf{I}_3 & 0_3 & 0_3 \\ -\hat{\mathbf{R}}_{k-1}(\hat{\mathbf{a}}_{k-1}^{\text{ext}}) \times dt & \mathbf{I}_3 & 0_3 \\ 0_3 & I_3 dt & I_3 \end{pmatrix} \mathbf{e}_{k-1} \\ & + \begin{pmatrix} 0_3 \\ \hat{\mathbf{R}}_{k-1} dt \\ 0_3 \end{pmatrix} \mathbf{e}_{k-1}^{a} + \begin{pmatrix} \mathbf{I}_3 dt \\ 0_3 \\ 0_3 \end{pmatrix} \mathbf{w}_{k-1}^{\omega}. \end{aligned}$$

By employing the equations of $\mathbf{A}_{k-1}$, $\mathbf{E}_{k-1}$, and $\mathbf{G}$ defined in the step 4 of Algorithm 1, we conclude:

$$\mathbf{e}_{k|k-1} = (\mathbf{A}_{k-1} \quad \mathbf{E}_{k-1}) \begin{pmatrix} \mathbf{e}_{k-1} \\ \mathbf{e}_{k-1}^{a} \end{pmatrix} + \mathbf{G} \mathbf{w}_{k-1}^{\omega}. \quad (13)$$

Thus, $E(\mathbf{e}_{k|k-1}) = \mathbf{0}$ follows from the unbiasedness of $\mathbf{e}_{k-1}$, $\mathbf{e}_{k-1}^a$ and $\mathbf{w}_{k-1}^\omega$. Finally, computing the covariance matrix $E(\mathbf{e}_{k|k-1} \mathbf{e}_{k|k-1}^T)$ confirms step 4 of Algorithm 1. ■

## B. External acceleration estimation

**Lemma 5.3:** Let $\mathbf{e}_{k|k-1}$ be unbiased, then the term $\tilde{\mathbf{y}}_k = \mathbf{y}_k - h(\hat{\mathbf{R}}_{k|k-1}, \hat{\mathbf{x}}_{k|k-1})$ has first order approximation $\tilde{\mathbf{y}}_k = \mathbf{D}\mathbf{a}_k^{\text{ext}} + \tilde{\mathbf{e}}_k$, where

$$\tilde{\mathbf{e}}_k = \mathbf{H}_k \mathbf{e}_{k|k-1} + \mathbf{w}_k^y. \quad (14)$$

The random variable $\tilde{\mathbf{e}}_k$ has zero mean and positive definite covariance matrix $\tilde{\mathbf{R}}_k$. $\mathbf{H}_k$ and $\tilde{\mathbf{R}}_k$ are introduced in steps 5 and 6 of Algorithm 1, respectively.

*Proof:* $\tilde{\mathbf{y}}_k = h(\hat{\mathbf{R}}_k, \hat{\mathbf{x}}_k) - h(\hat{\mathbf{R}}_{k|k-1}, \hat{\mathbf{x}}_{k|k-1}) + \mathbf{D}\mathbf{a}_k^{\text{ext}} + \mathbf{w}_k^y$. We simplify the term $\tilde{h} = h(\hat{\mathbf{R}}_k, \hat{\mathbf{x}}_k)$ -

$$\begin{aligned} h(\hat{\mathbf{R}}_{k|k-1}, \hat{\mathbf{x}}_{k|k-1}) &= \begin{pmatrix} \mathbf{R}_k^T \mathbf{g} \\ \mathbf{R}_k^T \mathbf{m}_e \end{pmatrix} - \begin{pmatrix} \mathbf{R}_{k|k-1}^T \mathbf{g} \\ \mathbf{R}_{k|k-1}^T \mathbf{m}_e \end{pmatrix} = \begin{pmatrix} (\exp_m(-\xi_{k|k-1}) - I_3)\mathbf{R}_{k|k-1}^T \mathbf{g} \\ (\exp_m(-\xi_{k|k-1}) - I_3)\mathbf{R}_{k|k-1}^T \mathbf{m}_e \end{pmatrix}, \quad \text{applying (1) we obtain} \end{aligned}$$

$$\tilde{h} = \begin{pmatrix} -(\xi_{k|k-1}) \times \hat{\mathbf{R}}_{k|k-1}^T \mathbf{g} \\ -(\xi_{k|k-1}) \times \hat{\mathbf{R}}_{k|k-1}^T \mathbf{m}_e \end{pmatrix} = \begin{pmatrix} (\hat{\mathbf{R}}_{k|k-1}^T \mathbf{g}) \times \xi_{k|k-1} \\ (\hat{\mathbf{R}}_{k|k-1}^T \mathbf{m}_e) \times \xi_{k|k-1} \end{pmatrix} = \mathbf{H}_k \mathbf{e}_{k|k-1} + \mathbf{w}_k^y = \mathbf{D}\mathbf{a}_k^{\text{ext}} + \tilde{\mathbf{e}}_k.$$

Substituting it gives $\tilde{\mathbf{y}}_k = \mathbf{D}\mathbf{a}_k^{\text{ext}} + \mathbf{H}_k \mathbf{e}_{k|k-1} + \mathbf{w}_k^y = \mathbf{D}\mathbf{a}_k^{\text{ext}} + \tilde{\mathbf{e}}_k$. Also, $E(\tilde{\mathbf{e}}_k) = \mathbf{0}$ follows from the unbiasedness of $\mathbf{e}_{k|k-1}$ and $\mathbf{w}_k^y$. Finally, computing the matrix $\tilde{\mathbf{R}}_k = E(\tilde{\mathbf{e}}_k \tilde{\mathbf{e}}_k^T)$ confirms step 6 of Algorithm 1. ■

**Theorem 5.4:** Let $\mathbf{e}_{k|k-1}$ be unbiased, then the first order approximation-based estimator of $\mathbf{a}_k^{\text{ext}}$ provided by step 8 of Algorithm 1 is unbiased minimum variance, with estimation error equals to

$$\mathbf{e}_k^a = \mathbf{a}_k^{\text{ext}} - \hat{\mathbf{a}}_k^{\text{ext}} = -\mathbf{M}_k \tilde{\mathbf{e}}_k, \quad (15)$$

and step 9 providing its estimation error covariance matrix.

*Proof:* Lemma 5.3 holds, thus $\tilde{\mathbf{y}}_k = \mathbf{D}\mathbf{a}_k^{\text{ext}} + \tilde{\mathbf{e}}_k$, where $\tilde{\mathbf{e}}_k$ is a random variable with zero mean and positive

definite covariance matrix $\tilde{\mathbf{R}}_k$, and the matrix $\mathbf{D}$ is full column rank, thus we can apply Gauss-Markov Theorem on $\tilde{\mathbf{y}}_k$ [8, Chapter 3.4.2], and the gain matrix $\mathbf{M}_k$ defined in step 7 of Algorithm 1 is making the estimator of step 8 unbiased minimum variance. This gain matrix satisfies $\mathbf{M}_k \mathbf{D} = \mathbf{I}_3$, applying it in the estimator of $\mathbf{a}_k^{\text{ext}}$ (after substituting $\tilde{\mathbf{y}}_k$), we obtain $\hat{\mathbf{a}}_k^{\text{ext}} = \mathbf{M}_k (\mathbf{D}\mathbf{a}_k^{\text{ext}} + \tilde{\mathbf{e}}_k) = \mathbf{a}_k^{\text{ext}} + \mathbf{M}_k \tilde{\mathbf{e}}_k$, thus $\mathbf{e}_k^a = \mathbf{a}_k^{\text{ext}} - \hat{\mathbf{a}}_k^{\text{ext}} = -\mathbf{M}_k \tilde{\mathbf{e}}_k$. Finally, computing the covariance matrix $\mathbf{P}_k^a = E(\mathbf{e}_k^a \mathbf{e}_k^T)$ confirms step 9 of Algorithm 1. ■

## C. Correction

**Lemma 5.5:** Let $\mathbf{e}_{k|k-1}$ be unbiased, then the first-order approximation of the estimation error of steps 12-14 of Algorithm 1 is as follows:

$$\begin{aligned} \mathbf{e}_k = & (\mathbf{I}_9 - \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{H}_k) \mathbf{e}_{k|k-1} \\ & - \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{w}_k^y \end{aligned} \quad (16)$$

*Proof:* Lemma 5.3 and Theorem 5.4 hold, thus $\mathbf{y}_k - h(\hat{\mathbf{R}}_{k|k-1}, \mathbf{x}_{k|k-1}) - \mathbf{D}\hat{\mathbf{a}}_k^{\text{ext}} = \mathbf{D}\mathbf{a}_k^{\text{ext}} + \tilde{\mathbf{e}}_k - \mathbf{D}\hat{\mathbf{a}}_k^{\text{ext}} = \mathbf{D}\mathbf{e}_k^a + \tilde{\mathbf{e}}_k = (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\tilde{\mathbf{e}}_k$. We define $\mathbf{K}_k^\xi = \mathbf{K}_k(1:3)$, $\mathbf{K}_k^v = \mathbf{K}_k(4:6)$, and $\mathbf{K}_k^x = \mathbf{K}_k(7:9)$, thus the steps 12-14 of Algorithm 1 can be written as:

$$\begin{aligned} \hat{\mathbf{R}}_k &= \hat{\mathbf{R}}_{k|k-1} \exp_m(\mathbf{K}_k^\xi (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k), \\ \hat{\mathbf{v}}_k &= \hat{\mathbf{v}}_{k|k-1} + \mathbf{K}_k^v (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k, \\ \hat{\mathbf{x}}_k &= \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k^x (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k. \end{aligned}$$

Attitude estimation error is given by:

$$\begin{aligned} \exp_m(\xi_k) &= \hat{\mathbf{R}}_k^{-1} \mathbf{R}_k, \\ &= \exp_m(-\mathbf{K}_k^\xi (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k) \hat{\mathbf{R}}_k^{-1} \mathbf{R}_k, \\ &= \exp_m(-\mathbf{K}_k^\xi (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k) \exp_m(\xi_k | k-1), \end{aligned}$$

applying BCH formula (2) gives $\xi_k = -\mathbf{K}_k^\xi (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k + \xi_k | k-1$. Velocity estimation error is given by:

$$\begin{aligned} \mathbf{e}_k^v &= \mathbf{v}_k - \hat{\mathbf{v}}_k, \\ &= \mathbf{v}_k - \hat{\mathbf{v}}_{k|k-1} - \mathbf{K}_k^v (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k, \\ &= \mathbf{e}_k^v | k-1 - \mathbf{K}_k^v (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k. \end{aligned}$$

Similar calculation for position gives $\mathbf{e}_k^x = \mathbf{e}_k^x | k-1 - \mathbf{K}_k^x (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k$. By vertically concatenating the three estimation errors, we obtain $\mathbf{e}_k = \mathbf{e}_k | k-1 - \mathbf{K}_k (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \tilde{\mathbf{e}}_k$. By substituting $\tilde{\mathbf{e}}_k = \mathbf{H}_k \mathbf{e}_{k|k-1} + \mathbf{w}_k^y$ (see Lemma 5.3) the proof is established. ■

**Lemma 5.6:** The terms $(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\tilde{\mathbf{R}}_k \mathbf{M}_k^T$ and $\mathbf{M}_k \tilde{\mathbf{R}}_k (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)^T$ are both zero.

*Proof:* The two terms are transposes of each other. Thus, proving that one is zero directly implies the other is also zero. $\mathbf{R}_k^* = (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\tilde{\mathbf{R}}_k \mathbf{M}_k^T = \tilde{\mathbf{R}}_k \mathbf{M}_k^T - \mathbf{D}\mathbf{M}_k \tilde{\mathbf{R}}_k \mathbf{M}_k^T$, substitute $\mathbf{M}_k$ from step 7 gives:

$$\begin{aligned} \mathbf{R}_k^* &= \tilde{\mathbf{R}}_k \tilde{\mathbf{R}}_k^{-1} \mathbf{D} (\mathbf{D}^T \tilde{\mathbf{R}}_k^{-1} \mathbf{D})^{-1} \\ &\quad - \mathbf{D} (\mathbf{D}^T \tilde{\mathbf{R}}_k^{-1} \mathbf{D})^{-1} \mathbf{D}^T \tilde{\mathbf{R}}_k^{-1} \tilde{\mathbf{R}}_k \tilde{\mathbf{R}}_k^{-1} \mathbf{D} (\mathbf{D}^T \tilde{\mathbf{R}}_k^{-1} \mathbf{D})^{-1}, \\ &= \mathbf{D} (\mathbf{D}^T \tilde{\mathbf{R}}_k^{-1} \mathbf{D})^{-1} - \mathbf{D} (\mathbf{D}^T \tilde{\mathbf{R}}_k^{-1} \mathbf{D})^{-1} = \mathbf{0}. \end{aligned}$$

■
**Lemma 5.7:** Let $e_{k|k-1}$ be unbiased, then step 15 of Algorithm 1 provides the covariance matrix $\mathbf{P}_k^{ea} = E(e_k e_k^{aT})$.

*Proof:* We compute the covarinace matrix $\mathbf{P}_k^{ea}$ employing (14), (15), and (16):

$$ 
\begin{aligned}
\mathbf{P}_k^{ea} &= E(\mathbf{e}_k \mathbf{e}_k^{aT}), \\
&= E(((\mathbf{I}_9 - \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{H}_k)\mathbf{e}_{k|k-1} \\
&\quad - \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{w}_k^y)(-\mathbf{M}_k\mathbf{H}_k\mathbf{e}_{k|k-1} - \mathbf{M}_k\mathbf{w}_k^y)^T), \\
&= -(\mathbf{I}_9 - \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{H}_k)\mathbf{P}_{k|k-1}^e \mathbf{H}_k^T \mathbf{M}_k^T \\
&\quad + \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{R}_k \mathbf{M}_k^T, \\
&= -\mathbf{P}_{k|k-1}^e \mathbf{H}_k^T \mathbf{M}_k^T \\
&\quad + \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\left(\mathbf{H}_k \mathbf{P}_{k|k-1}^e \mathbf{H}_k^T + \mathbf{R}_k\right) \mathbf{M}_k^T, \\
&= -\mathbf{P}_{k|k-1}^e \mathbf{H}_k^T \mathbf{M}_k^T + \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\tilde{\mathbf{R}}_k \mathbf{M}_k^T.
\end{aligned}
$$

Applying Lemma 5.6 gives $\mathbf{P}_k^{ea} = -\mathbf{P}_{k|k-1}^e \mathbf{H}_k^T \mathbf{M}_k^T$.

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

**Theorem 5.8:** Let $e_{k|k-1}$ and $e_k^a$ be unbiased, then the state estimates in steps 12-14 of Algorithm 1 are unbiased minimum variance, with step 16 providing the estimation error covariance matrix.

*Proof:* Lemma 5.5 holds, thus (16) is satisfied, and $E(\mathbf{e}_k) = \mathbf{0}$ follows from the unbiasedness of $\mathbf{e}_{k|k-1}$ and $\mathbf{w}_k^y$. $E(\|\mathbf{e}_k\|^2) = \text{tr}(\mathbf{P}_k^e)$, thus our goal is to prove that the matrix gain $\mathbf{K}_k$ defined in step 10 of Algorithm 1 satisfies $\frac{\text{dtr}(\mathbf{P}_k^e)}{\text{d}\mathbf{K}_k} = \mathbf{0}$. First we compute $\mathbf{P}_k^e$:

$$ 
\begin{aligned}
\mathbf{P}_k^e &= E(\mathbf{e}_k \mathbf{e}_k^T) = (\mathbf{I}_9 - \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{H}_k) \mathbf{P}_{k|k-1}^e (\mathbf{I}_9 - \\
&\quad \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{H}_k)^T + \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{R}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)^T \mathbf{K}_k^T \\
&= \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\tilde{\mathbf{R}}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)^T \mathbf{K}_k^T - \mathbf{P}_{k|k-1}^e \mathbf{H}_k^T (\mathbf{I}_9 - \\
&\quad \mathbf{D}\mathbf{M}_k)^T \mathbf{K}_k^T - \mathbf{K}_k(\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)\mathbf{H}_k \mathbf{P}_{k|k-1}^e + \mathbf{P}_{k|k-1}^e.
\end{aligned}
\quad (17) $$

Applying Lemma 5.6 gives:

$$ 
\begin{aligned}
\mathbf{P}_k^e &= \mathbf{K}_k \tilde{\mathbf{R}}_k (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)^T \mathbf{K}_k^T - \mathbf{P}_{k|k-1}^e \mathbf{H}_k^T (\mathbf{I}_9 - \\
&\quad \mathbf{D}\mathbf{M}_k)^T \mathbf{K}_k^T - \mathbf{K}_k (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k) \mathbf{H}_k \mathbf{P}_{k|k-1}^e + \mathbf{P}_{k|k-1}^e,
\end{aligned}
\quad (18) $$

implementing matrix derivatives leads to $\frac{\text{dtr}(\mathbf{P}_k^e)}{\text{d}\mathbf{K}_k} = 2\mathbf{K}_k \tilde{\mathbf{R}}_k (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)^T - 2\mathbf{P}_{k|k-1}^e \mathbf{H}_k^T (\mathbf{I}_9 - \mathbf{D}\mathbf{M}_k)^T$, the matrix gain $\mathbf{K}_k$ defined in step 10 of Algorithm 1 satisfy $\frac{\text{dtr}(\mathbf{P}_k^e)}{\text{d}\mathbf{K}_k} = \mathbf{0}$. Substituting this $\mathbf{K}_k$ in (18) confirms step 16 of Algorithm 1.

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

# VI. SIMULATIONS WITH THEORETICAL EXAMPLE

## A. Comparison algorithms

PVA-SO(3) is compared with two algorithms, both based on cascaded approaches [19]. First, the attitude is estimated using MARG sensors, then, it is used to compute the external acceleration based on the accelerometer measurement model (6). This computed acceleration serves as an input to a Kalman filter applied to the velocity and position dynamics (4)-(5), with position measurements as the output (8).

The first algorithm uses the invariant extended Kalman filter (IEKF) [1] for attitude estimation, assuming that the accelerometer measures only gravity, ignoring external acceleration. The second uses IEKF combined with a threshold-based adaptive method [11], which adjusts the measurement noise

covariance matrix. Specifically, when external acceleration is detected, a higher acceleration measurement noise covariance matrix is assigned, reducing the algorithm's reliance on accelerometer measurements. External acceleration is detected when the difference between the norm of the accelerometer output and the gravity norm exceeds a threshold $\epsilon_a$. We set $\epsilon_a = 0.2 \text{ m/s}^2$ following [11]. In the remainder of this paper, we refer to these two algorithms as cascaded-IEKF and cascaded-IEKF with adaptation, respectively.

## B. Simulation setup

The results are presented from Monte Carlo simulations with 100 runs. Each run has a different noise sequence and different initial estimates for position and velocity. The initial attitude estimates are obtained using a slightly perturbed rotation estimate, computed via the static attitude algorithm TRIAD [14] based on the first accelerometer and magnetometer readings. Each run lasts 100 seconds with a sampling time of $dt = 0.01 \text{ s}$. The Earth's gravity and magnetic field are set as $g = (0, 0, 9.81)^T \text{ m/s}^2$ and $m_e = (0.23, 0.01, 0.41)^T \text{ Gauss}$, respectively. The true values of body angular velocity and external acceleration are given in Table I. The gyroscope, accelerometer, magnetometer, and position noises are set as zero-mean white Gaussian signals with standard deviations of $\sigma_\omega = 0.01 \text{ rad/s}$, $\sigma_a = 0.01 \text{ m/s}^2$, $\sigma_m = 0.005 \text{ Gauss}$, and $\sigma_p = 0.5 \text{ m}$, respectively. The initial velocity and position estimates are set as uniform distributions in the ranges $[-10, 10] \text{ m/s}$ and $[-100, 100] \text{ m}$ for each component, respectively.

<table><caption>TABLE I: The true angular velocity and external acceleration</caption><thead><tr><th></th><th>True angular velocity<br>(rad/s)</th><th colspan="2">True external acceleration<br>(m/s<sup>2</sup>)</th></tr></thead><tbody><tr><td>ω<sub>x</sub></td><td>2.0 cos (0.2πkdt)</td><td><b>a</b><sub>x</sub><sup>ext</sup></td><td>2.0 sin (0.5πkdt)α<sub>k</sub></td></tr><tr><td>ω<sub>y</sub></td><td>1.5 cos (0.6πkdt)</td><td><b>a</b><sub>y</sub><sup>ext</sup></td><td>1.0 sin (0.2πkdt)α<sub>k</sub></td></tr><tr><td>ω<sub>z</sub></td><td>1.0 cos (1.0πkdt)</td><td><b>a</b><sub>z</sub><sup>ext</sup></td><td>0.5 sin (0.1πkdt)α<sub>k</sub></td></tr></tbody></table>

To ensure a fair comparison, the applied external acceleration shown in Table I at each time step $k$ is scaled by $\alpha_k$, which can take 0 (no external acceleration) or 1 (with external acceleration). The sequence $(\alpha_k)_{0\le k}$ is generated as alternating phases of zeros and ones. Each phase is generated to have a random duration following a uniform distribution in the range $[0, 1 - \bar{\alpha}]$ seconds for the zero phase, and $[0, \bar{\alpha}]$ seconds for the one phase. The closer $\bar{\alpha}$ is to 1, the more external acceleration is present. Conversely, as $\bar{\alpha}$ approaches 0, the presence of external acceleration decreases.

## C. Simulation results

Table II presents the root mean square error (RMSE) of PVA and external acceleration estimation, for the three algorithms across three scenarios of external acceleration presence, corresponding to $\bar{\alpha} = 1$, $\bar{\alpha} = 0.6$, and $\bar{\alpha} = 0.3$. In all scenarios, PVA-SO(3) outperforms the cascaded IEKF with and without adaptation and remains unaffected by the presence of external acceleration, unlike the cascaded IEKF approaches. In every scenario, the cascaded-IEKF with adaptation consistently outperforms the cascaded-IEKF without
adaptation. This is because the latter relies on unreliable acceleration measurements during the correction step when external acceleration is present, while the former neglects these measurements when detecting external acceleration. On the other hand, PVA-SO(3) demonstrates even better performance than the cascaded-IEKF with adaptation, as it fully exploits all available measurements, even in the presence of external acceleration. Fig. 1 and Fig. 2 show a comparison of velocity and position estimation, respectively, for a single simulation run, presenting the estimates from the three algorithms and the ground truth. These figures are appropriately zoomed in for better visualization. Both figures are consistent with the RMSE-based comparison shown in Table II.

<table><caption>TABLE II: RMSE for the rotation (in degree), velocity (in m/s), position (in m), and the external acceleration (EA) (in m/s²), for the three algorithms, over three $\alpha$ cases.</caption><thead><tr><th>α̂</th><th>State</th><th>PVA-SO(3)</th><th>cascaded-IEKF with adaptation</th><th>cascaded-IEKF</th></tr></thead><tbody><tr><td rowspan="4">1.0</td><td>Rotation</td><td>0.39</td><td>7.64</td><td>9.90</td></tr><tr><td>Velocity</td><td>0.06</td><td>1.01</td><td>1.82</td></tr><tr><td>Position</td><td>0.08</td><td>2.55</td><td>7.30</td></tr><tr><td>EA</td><td>0.02</td><td>0.54</td><td>0.69</td></tr><tr><td rowspan="4">0.6</td><td>Rotation</td><td>0.39</td><td>1.87</td><td>6.40</td></tr><tr><td>Velocity</td><td>0.06</td><td>0.34</td><td>1.09</td></tr><tr><td>Position</td><td>0.08</td><td>1.36</td><td>4.30</td></tr><tr><td>EA</td><td>0.02</td><td>0.20</td><td>0.48</td></tr><tr><td rowspan="4">0.3</td><td>Rotation</td><td>0.39</td><td>1.15</td><td>3.78</td></tr><tr><td>Velocity</td><td>0.06</td><td>0.17</td><td>0.51</td></tr><tr><td>Position</td><td>0.08</td><td>0.66</td><td>2.06</td></tr><tr><td>EA</td><td>0.02</td><td>0.12</td><td>0.28</td></tr></tbody></table>

<figure><img src="image_4.png" /><figcaption>Fig. 1: Velocity estimation comparison.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Fig. 2: Position estimation comparison.</figcaption></figure>

VII. CONCLUSION AND FUTURE WORK

This paper proposed a novel algorithm, PVA-SO(3), for estimating the position, velocity, and attitude of a rigid body using position and MARG sensor measurements. PVA-SO(3) treats external acceleration, which is measured by the accelerometer along with gravity and appears in the kinematic equation of velocity, as an unknown input affecting both the dynamic model and the output function. The effectiveness of PVA-SO(3) is demonstrated through Monte Carlo simulations and compared with a cascaded approach, where attitude is first estimated using the invariant extended Kalman filter (IEKF), and an IEKF integrated with threshold-based adaptation, then used for position and velocity estimation. Simulation results showed that PVA-SO(3) outperforms the cascaded methods.

REFERENCES

[1] A. Barrau and S. Bonnabel. Invariant Kalman filtering. *Annu. Rev. Control, Robot., Auton. Syst.*, 1(1):237–257, 2018.

[2] S. Berkane, A. Tayebi, and S. De Marco. A nonlinear navigation observer using IMU and generic position information. *Automatica*, 127:109513, 2021.

[3] J. L. Crassidis, F. L. Markley, and Y. Cheng. Survey of nonlinear attitude estimation methods. *J. Guid. Control Dyn.*, 30(1):12–28, 2007.

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

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

[6] M. S. Grewal, L. R. Weill, and A. P. Andrews. *Global Positioning Systems, Inertial Navigation, and Integration*. Wiley, 1 edition, 2007.

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

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

[9] R. Mahony, T. Hamel, and J.-M Pflimlin. Nonlinear complementary filters on the special orthogonal group. *IEEE Trans. Autom. Control*, 53(5):1203–1218, 2008.

[10] P. Martin and E. Salaün. Design and implementation of a low-cost observer-based attitude and heading reference system. *Control Eng. Pract.*, 18(7):712–722, 2010.

[11] A. M. Sabatini. Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing. *IEEE Trans. Biomed. Eng.*, 53(7):1346–1356, 2006.

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

[13] G. Shaaban, H. Fourati, A. Kibangou, and C. Prieur. MARG sensor-based attitude estimation on SO(3) under unknown external acceleration. *IEEE Control Syst. Lett.*, 7:3795–3800, 2023.

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

[15] Y. S. Suh. Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration. *IEEE Trans. Instrum. and Meas.*, 59(12):3296–3305, 2010.

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

[17] D. H. Titterton and J. L. Weston. *Strapdown inertial navigation technology*. Number 17 in IEE radar, sonar, navigation, and avionics series. Institution of Electrical Engineers, Stevenage, 2nd ed edition, 2004.

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

[19] Z. Zhou and J. Wu. Cascaded indirect kalman filters for land-vehicle attitude estimation with MARG sensors and GNSS observations. *IEEE Trans. Veh. Technol.*, 70(4):3267–3282, 2021.