Journal of Positioning, Navigation, and Timing (J Position Navig Timing; JPNT)
Abenezer Zegeye Woldeyohannis1
, Nak Yong Ko2†
1Smart Vehicle System Engineering, Chosun University, Gwangju 61452, Republic of Korea
2Department of Electronic Engineering, Chosun University, Gwangju 61452, Republic of Korea
†Corresponding Author: Nak Yong Ko, E-mail: nyko@chosun.ac.kr
Citation: Woldeyohannis, A. Z., & Ko, N. Y. 2025, Attitude Estimation on SO(3) Using MARG Data: A Lie Group Kalman Filter Approach, Journal of Positioning, Navigation, and Timing, 14, 341-351.
Journal of Positioning, Navigation, and Timing (J Position Navig Timing) 2025 December, Volume 14, Issue 4, pages 341-351. https://doi.org/10.11003/JPNT.2025.14.4.341
Received on Aug 17, 2025, Revised on Oct 17, 2025, Accepted on Nov 11, 2025, Published on Dec 15, 2025.
Copyright © The Institute of Positioning, Navigation, and Timing
License: Creative Commons Attribution Non-Commercial License (https://creativecommons.org/licenses/by-nc/4.0/) which permits unrestricted non-commercial use, distribution, and reproduction in any medium, provided the original work is properly cited.
This paper presents a Lie Group Extended Kalman Filter (LGEKF) for attitude estimation, formulated directly on the special orthogonal group SO(3), to preserve the geometric properties of 3D rotations. Unlike conventional methods that suffer from singularities or normalization issues, our approach leverages an exponential map to propagate rotations and fuses the Magnetometer, Accelerometer, Rate Gyroscope (MARG) sensor data while respecting the manifold structure of the SO(3). Using a dynamically challenging helical trajectory with realistic sensor noise and biases, we demonstrate that the LGEKF significantly outperforms the standard Extended Kalman Filter (EKF), reducing Root Mean Square (RMS) errors by 26.7% in yaw (0.44° vs. 0.60°), 99.48% in pitch (0.007° vs. 1.34°), and 99.84% in roll (0.008° vs. 5.15°). The covariance propagation of the filter remained stable even during aggressive maneuvers, reflecting its robustness in highly dynamic scenarios. The simulation results highlight the superiority of the LGEKF in drift mitigation and estimation efficiency, making it ideal for real-time applications in autonomous navigation vehicles in underwater environments and other Global Positioning System (GPS)denied environments.
Lie group, EKF, MARG, attitude estimation, SO(3)
Accurate attitude estimation is a cornerstone of navigation and control in autonomous vehicles, spanning a diverse array of platforms, such as aerial drones (Sabet et al. 2018), underwater vehicles (Kim et al. 2011), and spacecraft (Zhu et al. 2024). As these systems increasingly operate in complex environments that are often devoid of reliable Global Positioning System (GPS) signals, the need for robust and drift-resistant estimation techniques has become critical (Alghamdi et al. 2025). These techniques ensure operational integrity and enhance the safety and reliability of autonomous systems during high-stakes missions.
Traditional methods for representing orientation, such as Euler angles, are plagued with limitations, including singularities such as gimbal locks, which can abruptly hinder navigation capabilities (Jwo 2021). In contrast, quaternion-based approaches offer a solution by avoiding these singularities; however, they introduce complications, such as the necessity for normalization and ambiguities in sign representation (Zhang et al. 2018). Such challenges highlight the need for alternative methods that are geometrically consistent and mathematically rigorous (Shi et al. 2024).
One promising avenue lies in the utilization of the Lie group theory, particularly through the Special Orthogonal Group SO(3), which mathematically represents 3D rotation matrices (D’Eleuterio & Barfoot 2022). This framework not only maintains the essential properties of rotations, such as orthonormality and determinant constraints (Sol’a et al. 2018), but also eliminates the need for ad-hoc normalization typical of quaternion-based filters. The ability to handle rotations without singularities makes SO(3) particularly advantageous in highly dynamic scenarios, where linear approximations often fail to encapsulate the nonlinear characteristics of rotational motion (Qin 2024).
As technology advances, the proliferation of low-cost Magnetometer, Accelerometer, and Rate Gyroscope (MARG) sensors such as MEMS-based Inertial Measurement Units (IMUs) has intensified the demand for robust attitude estimation algorithms (Hyyti & Visala 2015). These sensors, which combine accelerometers, gyroscopes, and magnetometers, provide a wealth of data that, when accurately processed, can significantly enhance navigation capabilities (Chu et al. 2017). Although Kalman filters remain the widely accepted gold standard for state estimation, their application to Lie groups requires meticulous attention to the state propagation and measurement models to maintain accuracy and reliability (Liu et al. 2023).
Prior research has explored various approaches, including invariant extended Kalman filters (IEKFs) (Chauchat et al. 2017) and unscented filters (Daid et al. 2020), specifically designed for implementation in SO(3). The proposed Lie Group Extended Kalman Filter (LGEKF) aims to bridge this gap by combining geometric invariance with enhanced nonlinear estimation fidelity.
Our study aims to present a comprehensive framework that enhances the robustness and accuracy of attitude estimation techniques.
1) Formulating the attitude estimation problem in Lie group, in the framework of Extended Kalman filter.
2) Explicit derivation of Jacobian matrices of the process model and measurement model.
3) The filter’s performance is validated with noisy synthetic datasets, achieving sub-degree Root Mean Square (RMS) errors in yaw, pitch, and roll).
4) Deriving a process model for SO(3) that directly integrates gyroscopic rates via the exponential map, avoiding linearization errors.
5) Proposing a measurement model that fuses synthetic accelerometer and magnetometer data while respecting the group’s geometry.
Lie group-based estimation has gained traction with the development of the invariant observer theory, which exploits symmetries in dynamical systems. For attitude estimation, the SO(3) formulations outperformed quaternions by preserving orthonormality and avoiding renormalization. Recent studies have shown that probabilistic filters on SO(3) handle uncertainty propagation more naturally, a feature that we leverage in our Kalman filter design (Aslam et al. 2025).
MARG sensor fusion has been extensively studied with lightweight solutions such as Madgwick’s gradient-descent filter and Mahony’s complementary filter (Hoang et al. 2021). However, these methods often lack robust uncertainty quantification provided by Kalman filters. Our approach combines the geometric fidelity of SO(3) with the optimality of Kalman filtering, effectively addressing the limitations of both domains. A recent study (Ko et al. 2022) further demonstrated the effectiveness of the Lie group approach in dynamic-model-aided navigation for multirotor UAVs, whereas (Jeong & Ko 2024) extended this framework to underwater vehicle navigation by compensating for sensor misalignment using the Lie theory.
Recent advances in state estimation of manifolds have demonstrated that utilizing the underlying Lie group structure of SO(3) leads to more stable and accurate filtering methods. The IEKF exploits system symmetries through invariant error formulations to achieve theoretically appealing properties such as state-independent Jacobians and improved consistency guarantees, and enhances stability and convergence by not relying on the direct linearization of nonlinear processes and measurement models (Ko et al. 2018). This approach has demonstrated superior performance compared with standard Extended Kalman Filters (EKFs) in various applications, particularly in visual-inertial odometry and robotic navigation, where the symmetry properties are pronounced (Barrau & Bonnabel 2014). Another emerging trend is the direct use of probabilistic filtering on Lie groups, which improves robustness under sensor drift and noise features that align closely with the demands of navigation in unstructured environments (Guo et al. 2023).
The widespread adoption of low-cost MARG sensors has created opportunities for robust attitude estimation with reduced costs and sizes (Wu et al. 2016). Although classical filtering methods offer lightweight solutions, they often struggle in environments with sensor degradation or aggressive motion.
The present work contributes to this landscape by proposing an LGEKF that adopts a philosophical approach different from the IEKF methodology. While the IEKF emphasizes theoretical invariance through group symmetries, our LGEKF focuses on geometric consistency through an explicit treatment of the SO(3) manifold structure. Our approach strikes a balance among geometric fidelity, estimation efficiency, and robustness, making it ideal for embedded systems and real-time applications.
The remainder of this paper is organized as follows. The methodology and outline of the main topic are presented in Section II. Section III presents and discusses our results. Finally, Section IV provides the conclusions and recommendations for future research.
In this section, the process and measurement models used in the LGEKF for attitude estimation are defined. The formulation is grounded in the Special Orthogonal Group SO(3), which accurately represents the 3D rotational motion without singularities or overparameterization. The sensor inputs included triaxial angular velocity, linear acceleration, and magnetic field measurements from the MARG sensor unit.
In this section, the problem statement of the research is outlined, and the mathematical symbols used throughout the study are detailed in Table 1.
Table 1. Symbols used in this paper.
| Symbol | Description |
|---|---|
| R(k) | Rotation matrix from body frame to inertial (NED) frame at time k; R(k) ∈ SO(3) |
| ω(k) | Angular velocity vector ∈ ℜ3, measured by gyroscope, expressed as [p(k),q(k),r(k)]T |
| p(k), q(k), r(k) | Roll, pitch, and yaw rates respectively at time k, measured in rad/s from the gyroscope |
| gn | Known gravity vector in the NED frame, typically [0,0,1]T |
| mn | Known, normalized magnetic field vector in the NED frame |
| ab(k) | Accelerometer output in the body frame at time k |
| mb(k) | Magnetometer output in the body frame at time k |
| ãb(k) | Normalized accelerometer vector |
| m̃b(k) | Normalized magnetometer vector |
| z(k) | Measurement vector, stacking both normalized sensor readings: z(k) ∈ ℜ6 |
| Δt | Sampling interval or time step |
| ⌊·⌋× | Skew-symmetric matrix operator for cross-product in SO(3) |
| R̂(k|k) | Estimation of attitude represented in SO(3) |
| F(k) | Jacobian of the process model with respect to the rotation state R(k). |
| H(k) | Jacobian of the measurement model with respect to the rotation state R(k). |
| Q(k) | Process noise covariance matrix |
| V(k) | Measurement noise covariance matrix |
| P(k) | Covariance matrix of the attitude estimation error |
| K(k) | Kalman gain matrix |
| e(k) | Innovation (residual) vector in the measurement update step |
| exp(·) | Exponential map |
| τ | Tangent-space perturbation induced by the gyroscope noise. |
| bg(t), ba(t), bm(t) | Time-varying biases for gyroscope, accelerometer, and magnetometer, respectively. |
| ηg(t), ηa(t), ηm(t) | Gaussian noise terms for gyroscope, accelerometer, and magnetometer, respectively. |
| σg(t), σa(t), σm(t) | Standard deviations of sensor noise for gyroscope, accelerometer, and magnetometer, respectively. |
Problem Statement
Objective: To estimate the 3D orientation (attitude) of a rigid body over time using MARG sensor data.
Given:
(i) Angular velocity from gyroscope,
(ii) Specific force from accelerometer,
(iii) Magnetic field from magnetometer.
Reference Frame: The inertial frame is defined as NED.
Approach: Employ a LGEKF formulated on SO(3) to recursively estimate the attitude represented by rotation matrix $R(k)$.
Output: $\hat{R}(k|k)$, an estimate of the orientation of the body at each time step $k$.
The state variable in our model is the rotation matrix R(k)∈SO(3) which maps vectors from the body-fixed frame to the inertial (world) frame. SO(3) group is defined as
$$\text{SO(3)} = \{ R \in R^{3 \times 3} | R^T R = I, \det(R) = +1 \}.$$
This matrix describes the orientation of the body frame with respect to the fixed reference frame. The reference frame in the proposed system is the NED frame. Accordingly, gravity is represented by
$$g_n = [0 \quad 0 \quad 1]^T$$
This implies that gravity acts downward along the positive z axis in the NED frame. The Earth’s magnetic field is denoted by $m \in R^3$, a known, normalized vector in the NED frame obtained from geomagnetic models.
Attitude dynamics are driven by the angular velocity vector measured in the body frame.
$$\omega(k) = \begin{bmatrix} p(k) \\ q(k) \\ r(k) \end{bmatrix}.$$
The continuous-time rotational motion is discretized using the exponential map from the Lie algebra SO(3), leading to a discrete-time attitude update:
$$R(k + 1) = R(k) \exp(\lfloor \omega(k) \rfloor_{\times} \Delta t)$$
Process Noise Incorporation: The process noise enters through gyroscopic measurements during state propagation. Considering the noisy angular velocity measurement $\tilde{\omega}(k) = \omega(k) + w_g(k)$ where $w_g(k) \sim N(0, Q_g)$ is zero-mean Gaussian noise, the discrete-time process model accounts for uncertainty as shown in Eq. (4). The process noise covariance $Q(k)$ in the filter represents the linearized effect of $w_g (k)$ on the state in the tangent space of SO(3).
The skew-symmetric matrix $\lfloor \omega(k) \rfloor_{\times}$ is given by:
$$\lfloor \omega(k) \rfloor_{\times} =
\begin{bmatrix}
0 & -r(k) & q(k) \\
r(k) & 0 & -p(k) \\
-q(k) & p(k) & 0
\end{bmatrix}$$
This matrix encodes the cross-product operation, and when used in the exponential map (Eq. (4)) ensures that the updated rotation $R(k+1)$ remains on SO(3), preserving the orthonormality and avoiding drift.
The measurement model fuses normalized accelerometer and magnetometer data to estimate the vehicle orientation. These sensor readings were expressed in the body frame and mapped to known reference vectors in the inertial (navigation) frame.
At each time step, the raw accelerometer and magnetometer outputs are:
$$a_b(k) =
\begin{bmatrix}
a_x(k) \\
a_y(k) \\
a_z(k)
\end{bmatrix},
m_b(k) =
\begin{bmatrix}
m_x(k) \\
m_y(k) \\
m_z(k)
\end{bmatrix}$$
Normalized accelerometer ($a_b(k)$) and magnetometer ($m_b(k)$) measurements are:
$$\tilde{a}_b(k) = \frac{a_b(k)}{\|a_b(k)\|}, \tilde{m}_b(k) = \frac{m_b(k)}{\|m_b(k)\|}$$
The full measurement vector is:
$$z(k) =
\begin{bmatrix}
\tilde{a}_b(k) \\
\tilde{m}_b(k)
\end{bmatrix} \in R^6$$
The normalized inertial vectors are projected to the body frame:
$$z(k) =
\begin{bmatrix}
-R^T(k)g_n \\
R^T(k)m_n
\end{bmatrix} \in R^6$$
The LGKF requires linearizing nonlinear models around the current estimate. The perturbations are expressed in the tangent space SO(3). Unlike the IEKF, which exploits symmetry to achieve state-independent Jacobians, our approach derives explicit state-dependent Jacobians through a perturbation analysis, providing mathematical transparency while maintaining geometric consistency.
The state-transition Jacobian reuses Eq. (4)’s Exponential map. Its Jacobian with respect to $R(k)$ is
$$F(k+1) = \frac{\partial R(k+1)}{\partial R(k)} = \{\exp(\lfloor \omega(k) \rfloor_{\times} \Delta t)\}^T$$
This ensures the rotation matrix $R(k+1)$ remains properly constrained on SO(3).
Using the identity in Eq. (9), the Jacobian becomes:
$$\frac{\partial (R^T v)}{\partial R} = R^T \lfloor v \rfloor_{\times} R$$
Consequently, the Jacobian can be represented as:
$$H(k) =
\begin{bmatrix}
-R^T(k|k) \lfloor g_n \rfloor_{\times} R(k|k) \\
R^T(k|k) \lfloor m_n \rfloor_{\times} R(k|k)
\end{bmatrix} \in R^{6 \times 3}$$
The LGEKF operates through the following prediction and correction steps:
Prediction:
$$\hat{R}(k + 1|k) = \hat{R}(k|k) \exp(\lfloor \omega(k) \rfloor_{\times} \Delta t)$$
$$P(k + 1|k) = F(k)P(k|k)F^T(k) + Q(k)$$
Correction:
$$K(k + 1) = P(k + 1|k)H^T(k + 1) [H(k + 1)P(k + 1|k)H^T(k + 1) + V]^{-1}$$
Innovation:
$$e(k + 1) = z(k + 1) – h(\hat{R}(k + 1|k))$$
State Update:
$$\hat{R}(k + 1|k + 1) = \hat{R}(k + 1|k) \exp(\lfloor K(k + 1)e(k + 1) \rfloor_{\times})$$
Covariance Update:
$$\begin{align}
P(k + 1|k + 1) = &(I – K(k + 1)H(k + 1))P(k + 1|k) \notag \\
&(I – K(k + 1)H(k + 1))^T \notag \\
&+ K(k + 1)RK^T(k + 1)
\end{align}$$
This formulation ensures that attitude estimation remains consistent with the geometry of SO(3), enabling robust and accurate performance even in highly dynamic environments.
A critical aspect of applying a Kalman filter to a manifold is the proper definition of the estimation error. Following the Lie theory framework (Sol’a et al. 2018), we define a right-invariant error that measures the discrepancy of the estimate from the truth. The right-invariant estimation error is defined by using the following convention:
$$\delta \theta(k) = \hat{R}(k|k) \ominus R_{\text{true}}(k) = \log \{ R_{\text{true}}(k)^{-1} \hat{R}(k|k) \}$$
where $\delta \theta(k) \in R^3$ represents the error vector in the Lie algebra SO(3). The error covariance matrix $P(k)$ is then defined as
$$P(k) = E[\delta \theta(k) \, \delta \theta^T(k)]$$
The process noise covariance $Q(k)$ represents the covariance of the gyroscope noise vector $w_g(k)$. The gyroscope noise enters the rotation propagation through the exponential map, producing a perturbation in the Lie algebra tangent space. Therefore, the process noise covariance in the Lie algebra is
$$Q(k) = E[\tau \tau^T]$$
where $\tau \in R^3$represents the tangent-space perturbation induced by the gyroscope noise.
This section presents a thorough validation of the proposed LGEKF through comprehensive simulations with emphasis on attitude estimation accuracy. To establish a robust benchmark, we compared the LGEKF with the conventional EKF, which is a widely recognized and extensively utilized baseline in nonlinear state estimation.
The EKF operates as a conventional filter for attitude estimation and lacks sophisticated techniques integrated within the LGEKF. Our analysis focuses on the precision of the attitude estimation results, highlighting the advantages of the proposed method. The results illustrated in the accompanying figures demonstrate the comparative performance of both methods across various scenarios.
To rigorously evaluate the performance of the proposed LGEKF against that of the conventional EKF, we generated synthetic sensor data based on a helical trajectory (Fig. 1), which is a dynamic motion profile that combines continuous rotation and translation. The corresponding sensor measurements, including the gyroscope, accelerometer, and magnetometer data, were corrupted with realistic MEMS noise and biases to emulate low-cost MARG sensor behavior under demanding motion conditions. This trajectory challenges filters with time-varying angular velocities and accelerations, mimicking real-world scenarios, such as autonomous underwater vehicle (AUV) maneuvers.
Fig. 1. Synthetically generated trajectory and sensor data.
The true attitude motion follows a 3D helical path defined by:
The synthetic measurements for the gyroscope and accelerometer were modeled with realistic noise and bias characteristics, emulating the low-cost MEMS sensors commonly used in practical applications.
1) Gyroscope model:
The measured angular velocity $\tilde{\omega}(t)$ is corrupted by time-varying bias $b_g(t)$ and Gaussian noise $\eta_g(t)$:
$$\tilde{\omega}(t) = \omega(t) + b_g(t) + \eta_g(t)$$
where $\omega(t)$ is true angular velocity (in rad/s), $b_g(t)$ is bias modeled as a random walk process given by $b_g(t) = \eta b_g(t)$, where $\eta b_g(t)$ follows a normal distribution $N(0, \sigma b_g^2)$, and $\eta _g(t)$ is zero-mean Gaussian noise which follows $N(0, \sigma _g^2)$.
2) Accelerometer model:
The specific force measurement $\tilde{a}(t)$ includes gravity, bias, and noise:
$$\tilde{a}(t) = R^T(t)(g + b_a(t) + \eta_a(t))$$
where $R(t)$ is the rotation matrix that transforms from the body frame to the world frame, $g = [0 \quad 0 \quad -9.81]^T$ m/s2 is the gravity vector, $b_a(t)$ is the accelerometer bias (constant or slowly varying), and $\eta_a(t)$ is the Gaussian noise following a normal distribution $N(0, \sigma _g^2)$.
3) Magnetometer model:
If simulating an AHRS, the magnetometer measures:
$$\tilde{m}(t) = R^T(t)m_{ref} + b_m(t) + \eta_m(t)$$
where $m_{ref} = [0.2 \quad 0 \quad 0.4]^T$ Gauss is reference earth magnetic field, $ b_m(t)$ is the magnetometer bias (constant or slowly varying), and $\eta_m(t)$ a Gaussian noise with distribution of $N(0, \sigma _m^2)$.
Noise and Bias Parameters: To reflect realistic sensor imperfections, the following parameters were adopted:
(a) Gyroscope
Noise ($\sigma _g$): 0.0035°/s/√Hz, Bias Instability: 10 /hr, Range: ±450°/s.
(b) Accelerometer
Noise ($\sigma _a$): 50 µg/√Hz, Bias Instability: 20 µg Range: ±160 m/s².
(c) Magnetometer
Noise ($\sigma _m$): 0.15 mGauss/√Hz, Bias Vector: [0.002, -0.003, 0.001] Gauss, Range: ±2.5 Gauss.
The process noise covariance Q = 3.73×10⁻⁹I₃ is derived from the gyroscope’s angular random walk of = 0.0035°/s/√Hz, converted to 6.11×10⁻⁴ rad/s at 100 Hz sampling rate using the relation Q = σg²Δt. The measurement noise covariance R = blkdiag (2.41×10⁻⁵I₃, 2.25×10⁻⁶I₃) incorporates the accelerometer velocity random walk of σa = 4.91×10⁻³ m/s² (from 50 µg/√Hz) and magnetometer noise of σm = 1.5×10⁻³ Gauss (from 0.15 mGauss/√Hz). The initial covariance P = 1.22×10⁻³I₃ corresponds to an initial attitude uncertainty of ±2° (1σ) in each Euler angle, reflecting typical static alignment accuracy with the XSENS MTi-G-710 specifications.
This section presents a comprehensive evaluation of the proposed LGEKF for attitude estimation, and compares its performance with that of the conventional EKF using synthetic MARG sensor data. The results were analyzed using three key metrics: attitude estimation accuracy, covariance trace, and estimation error, as illustrated in Figs. 2 and 3 and quantified in Table 2.
Fig. 2. Attitude estimation; (a) yaw, pitch, and roll estimation, (b) Covariance trace for yaw, pitch, and roll, (c) estimation error of yaw, pitch, and roll.
Fig. 3. Estimation statistical error.
Table 2. Comparison of attitude estimation error.
| Attitude error (deg) | Yaw | Pitch | Roll | |||
|---|---|---|---|---|---|---|
| EKF | LGEKF | EKF | LGEKF | EKF | LGEKF | |
| MN | 0.4616 | 0.35485 | 1.1749 | 0.0002942 | 4.1671 | 0.0046262 |
| STD | 0.37703 | 0.26732 | 0.63908 | 0.0068367 | 4.0362 | 0.0066115 |
| Min | -1.5859 | -0.97334 | -3.7105e-10 | -0.026141 | -11.227 | -0.030834 |
| Max | 0.054148 | 0.072937 | 2.1873 | 0.022682 | 3.7982 | 0.022099 |
| RMS | 0.596 | 0.44347 | 1.3374 | 0.0068429 | 5.1531 | 0.0080692 |
Fig. 2a compares the yaw, pitch, and roll estimation performances of the LGEKF and conventional EKF against the ground truth. Across all three axes, the LGEKF tracked the ground truth much more closely, maintaining small errors even during abrupt attitude changes and large rotation maneuvers. In yaw estimation, the LGEKF curve almost overlapped with the ground truth throughout the simulation, whereas the EKF exhibited significant deviations during high-dynamic periods. Similar trends were observed in the pitch and roll, where the EKF produced large overshoots and undershoots, especially during rapid orientation changes, whereas the LGEKF remained relatively stable. These results confirm that the geometry-preserving SO(3) formulation of the LGEKF mitigates the drift and linearization errors, enabling robust performance under both slow and fast rotational dynamics.
Fig. 2b depicts the evolution of the covariance traces for all three attitude components, representing the internal uncertainty estimates of the filter. The LGEKF maintained consistently low covariance values close to zero throughout the simulation, with smooth, stable traces that demonstrated well-calibrated uncertainty quantification. For the yaw and pitch components, the LGEKF showed a nearly constant covariance near zero, whereas for the roll component, both the LGEKF and EKF exhibited gradual increases in covariance over time. However, the roll covariance of the LGEKF remains significantly lower than that of the EKF throughout the entire trajectory, demonstrating better uncertainty management. This stable covariance propagation indicates that the Lie group formulation preserves consistent confidence levels despite the dynamic motion.
Fig. 2c quantifies the absolute estimation errors for yaw, pitch, and roll, presenting the time histories of the attitude estimation errors for both the LGEKF and the conventional EKF. The LGEKF demonstrated superior performance across all three axes, maintaining significantly smaller error magnitudes than the EKF throughout the entire simulation. For pitch and roll estimation, the LGEKF achieves exceptional accuracy with errors consistently near zero, demonstrating its ability to precisely track these critical attitude components. In contrast, the conventional EKF exhibited substantially larger error magnitudes and more pronounced oscillations. Overall, the figure underscores the capability of the LGEKF to provide a more accurate, stable, and reliable attitude estimation with a dramatically reduced error drift and fewer large deviations compared to the standard EKF formulation.
Table 2 and Fig. 3 demonstrate that the proposed LGEKF significantly outperforms the conventional EKF in terms of both the mean and RMS errors across all attitude components (yaw, pitch, and roll). For yaw estimation, the mean error decreased from 0.46° (EKF) to 0.36° (LGEKF), and the RMS error was reduced from 0.6° to 0.44°. Pitch estimation shows a similar improvement, with mean error decreasing from 1.18° to 0.0003°, and RMS error dropping from 1.34° to 0.007°. Roll estimation benefits even more, with mean error reducing from 4.17° to 0.005° and RMS error decreasing sharply from 5.15° to 0.008°. These RMS reductions indicate not only higher accuracy but also improved stability over time. Fig. 3 visually reinforces these results, showing consistently smaller bars for the LGEKF across all statistical metrics, indicating the enhanced stability, accuracy, and robustness of the proposed filter in attitude estimation.
The comparative analysis in Table 3 reveals a clear tradeoff between computational efficiency and estimation accuracy. The EKF demonstrates superior computational performance, operating 5.4 times faster than the LGEKF with a maximum update rate of 35.5 kHz, making it suitable for resource-constrained real-time applications. However, it exhibits significantly poorer attitude estimation accuracy, particularly in roll (4.17° error) and pitch (1.17° error), owing to Euler angle singularities and linearization limitations. In contrast, the LGEKF achieved exceptional estimation precision with sub-degree errors across all axes (0.56° RMSE) and near-perfect roll/pitch estimation (0.006-0.007° errors), leveraging its Lie group formulation to avoid singularity issues. While the LGEKF requires 3.04 of processing time primarily due of expensive matrix exponential operations (59.5% of the computation), its 8.8× improvement in overall accuracy and guaranteed group structure preservation make it a superior choice for applications demanding high-precision attitude estimation.
Table 3. General performance characteristics of EKF and LGEKF.
| Metric | EKF | LGEKF | Advantage |
|---|---|---|---|
| Computational performance | |||
| Total processing time | 0.5636 sec | 3.04 sec | EKF (5.4× faster) |
| Iteration time | 28 μs | 152 μs | EKF (5.4× faster) |
| Max update rate | 35485 Hz | 6,569 Hz | EKF (5.4× higher) |
| Accuracy & Error | |||
| Final RMSE | 4.91° | 0.56° | LGEKF (8.8× more accurate) |
| Mean roll error | 4.1671° | 0.007° | LGEKF (595× better) |
| Mean pitch error | 1.1749° | 0.006° | LGEKF (195× better) |
| Mean yaw error | 0.4652° | 0.36° | LGEKF (1.3× better) |
| Robustness & Stability | |||
| Singularity events | 0 | 0 | Both robust |
| Robustness score | 10.0/10 | 10.0/10 | Both perfect |
| Group structure | N/A | reserved | LGEKF |
| Stability assessment | Limited | Enhanced | LGEKF |
| Computational distribution | |||
| Prediction time | 6.8% | 37.0% | EKF (lighter prediction) |
| Update time | 93.2% | 63.0% | Balanced |
| Exponential operations | N/A | 59.5% | EKF (no exponentials) |
The computational performance reported in Table 3 was measured on a desktop computer with hardware configuration featured an Intel(R) Core(TM) i7-10700F CPU @ 2.90 GHz and 16.0 GB of RAM. The simulations were conducted using MATLAB R2024b.
To establish clarity and reproducibility, the performance metrics in Table 3 are defined as follows:
This study introduced an LGEKF for attitude estimation formulated on the special orthogonal group SO(3) and demonstrated its advantages over conventional methods through rigorous testing using synthetic MARG sensor data. The proposed approach successfully addresses critical challenges in attitude estimation by preserving the geometric properties of rotations through an exponential map and eliminating the singularities and normalization issues inherent in the Euler angle and quaternion representations.
The simulation results clearly demonstrate the superior performance of the LGEKF compared to the standard EKF. The proposed method maintains a higher accuracy with sub-degree RMS errors across all attitude angles, even during aggressive maneuvers, where traditional approaches typically degrade. Furthermore, the LGEKF exhibits a more stable covariance propagation, reflecting its ability to properly account for uncertainty under high-dynamic conditions. This robust performance stems from the filter foundation of Lie’s group theory, which ensures geometric consistency throughout the estimation process.
Several promising directions have emerged for extending this study. It is important to note that this study validated the filter under the assumption of negligible non-gravitational acceleration; therefore, a key future direction involves extending the LGEKF to explicitly model and compensate for motion-induced acceleration. Future research should investigate the performance of the filter with real sensor data and explore its integration with complementary navigation systems to enhance its robustness in challenging environments. Additional improvements could focus on adaptive tuning mechanisms and bias compensation techniques to further increase reliability. The mathematical rigor and computational efficiency of the LGEKF make it particularly suitable for implementation in resource-constrained autonomous systems, suggesting its broad applicability across aerospace, marine, and robotic applications.
This study was supported by research fund from Chosun University, 2025.
Conceptualization, N.Y.K.; methodology, N.Y.K. and W.A.Z.; software, W.A.Z.; validation, N.Y.K.; formal analysis, N.Y.K. and W.A.Z.; investigation, N.Y.K.; resources, N.Y.K.; data curation, N.Y.K. and W.A.Z.; writing—original draft preparation, W.A.Z.; writing—review and editing, N.Y.K.; visualization, W.A.Z.; supervision, N.Y.K.; project administration, N.Y.K.; and funding acquisition, N.Y.K. All authors have read and agreed to the published version of the manuscript.
The authors declare no conflict of interest.
Alghamdi, S., Alahmari, S., Yonbawi, S., Alsaleem, K., Ateeq, F., et al. 2025, Autonomous Navigation Systems in GPS-Denied Environments: A Review of Techniques and Applications, 11th International Conference on Automation, Robotics, and Applications (ICARA), Zagreb, Croatia, 2025, 290-299. https://doi.org/10.1109/ICARA64554.2025.10977619
Aslam, F., Haydar, M., & Akhtar, S. 2025, Geometric Nonlinear Filtering with Almost Global Convergence for Attitude and Bias Estimation on the Special Orthogonal Group, ArXiv. https://doi.org/10.48550/arXiv.2503.08255
Barrau, A. & Bonnabel, S. 2014, The Invariant Extended Kalman Filter as a Stable Observer, IEEE Transactions on Automatic Control, 62, 1797-1812. https://doi.org/10.1109/TAC.2016.2594085
Chauchat, P., Barrau, A., & Bonnabel, S. 2017, Kalman filtering with a class of geometric state equality constraints, 2017 IEEE 56th Annual Conference on Decision and Control (CDC). 2581-2586. https://doi.org/10.1109/CDC.2017.8264033
Chu, Z., Chen, C., Liu, Y., Wang, Y., & Lin, X. 2017, Magnetic orientation system based on magnetometer, accelerometer and gyroscope, CAAI Trans. Intell. Technol., 2, 166-172. https://doi.org/10.1049/TRIT.2017.0024
Daid, A., Busvelle, E., & Aidène, M. 2020, On the convergence of the unscented Kalman filter, Eur. J. Control, 57, 125-134. https://doi.org/10.1016/j.ejcon.2020.05.003
D’Eleuterio, G. & Barfoot, T. 2022, On the eigen structure of rotations and poses: commonalities and peculiarities, Proceedings of the Royal Society A, 478. https://doi.org/10.1098/rspa.2022.0080
Guo, H., Liu, H., Zhou, Y., & Hu, X. 2023, Robust State Estimation via Maximum Correntropy EKF on Matrix Lie Groups with Application to Low-Cost INS/GPS-Integrated Navigation System, IEEE Sensors Journal, 23, 9467-9479. https://doi.org/10.1109/JSEN.2023.3251389
Hoang, M., Iacono, S., Paciello, V., & Pietrosanto, A. 2021, Measurement Optimization for Orientation Tracking Based on No Motion No Integration Technique, IEEE Transactions on Instrumentation and Measurement, 70, 1-10. https://doi.org/10.1109/TIM.2020.3035571
Hyyti, H. & Visala, A. 2015, A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs, International Journal of Navigation and Observation, 2015, 1-18. https://doi.org/10.1155/2015/503814
Jeong, D. B. & Ko, N. Y. 2024, Sensor Fusion for Underwater Vehicle Navigation Compensating Misalignment Using Lie Theory, Sensors, 24, 1653. https://doi.org/10.3390/s24051653
Jwo, D. 2021, Estimation of Quaternion Motion for GPS-based Attitude Determination Using the Extended Kalman Filter, Computers, Materials & Continua, 66, 2105-2126. https://doi.org/10.32604/cmc.2020.014241
Kim, H.-S., Choi, H.-S., Yoon, J. -S., & Ro, P. I. 2011, Study on AHRS Sensor for Unmanned Underwater Vehicle, International Journal of Ocean System Engineering, 1, 165-170. https://doi.org/10.5574/IJOSE.2011.1.3.165
Ko, N. Y., Song, G., Youn, W., & You, S. H. 2022, Lie Group Approach to Dynamic-Model-Aided Navigation of Multirotor Unmanned Aerial Vehicles, in IEEE Access, 10, 72717-72730. https://doi.org/10.1109/ACCESS.2022.3180769
Ko, N. Y., Youn, W., Choi, I. H., Song, G., & Kim, T. S. 2018, Features of Invariant Extended Kalman Filter Applied to Unmanned Aerial Vehicle Navigation, Sensors, 18, 2855. https://doi.org/10.3390/s18092855
Liu, B., Chen, H., & Zhang, W. 2023, A General Iterative Extended Kalman Filter Framework for State Estimation on Matrix Lie Groups, 2023 62nd IEEE Conference on Decision and Control (CDC), 1177-1182. https://doi.org/10.1109/CDC49753.2023.10383770
Qin, M. 2024, The Transformative Role of Group Theory in Physics, Highlights in Science, Engineering and Technology. https://doi.org/10.54097/7cekz543
Sabet, M., Daniali, H., Fathi, A., & Alizadeh, E. 2018, A Low-Cost Dead Reckoning Navigation System for an AUV Using a Robust AHRS: Design and Experimental Analysis. IEEE Journal of Oceanic Engineering, 43, 927-939. https://doi.org/10.1109/JOE.2017.2769838
Sol’a, J., Deray, J., & Atchuthan, D. 2018, A micro Lie theory for state estimation in robotics, arXiv. https://doi.org/10.48550/arXiv.1812.01537
Shi, Z. & Chen, G. 2024, Coq Formalization of Orientation Representation: Matrix, Euler Angles, Axis-Angle and Quaternion, 79-96. https://doi.org/10.1007/978-3-031-71261-6_5
Wu, J., Zhou, Z., Chen, J., Fourati, H., & Li, R. 2016, Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors, IEEE Sensors Journal, 16, 6997-7007. https://doi.org/10.1109/JSEN.2016.2589660
Zhang, G., Hu, B., Chang, L., & Xue, B. 2018, Nonlinear Initial Alignment based on Quaternion Error Model, 2018 IEEE CSAA Guidance, Navigation and Control Conference (CGNCC), 1-6. https://doi.org/10.1109/GNCC42960.2018.9019158
Zhu, H., Gui, H., & Zhong, R. 2024, Unscented Schmidt-Kalman filter on Lie groups with application to spacecraft attitude estimation, Advances in Space Research, 74, 5713-5724. https://doi.org/10.1016/j.asr.2024.08.035