Three-Dimensional Dead-Reckoning Based on Lie Theory for Overcoming Approximation Errors

Abstract

This paper proposes a dead-reckoning (DR) method for vehicles using Lie theory. This approach treats the pose (position and attitude) and velocity of the vehicle as elements of the Lie group ${\mathit{SE}}_2\left(3\right)$ and follows the computations based on Lie theory. Previously employed DR methods, which have been widely used, suffer from cumulative errors over time due to inaccuracies in the calculated changes from velocity during the motion of the vehicle or small errors in modeling assumptions. Consequently, this results in significant discrepancies between the estimated and actual positions over time. However, by treating the pose and velocity of the vehicle as elements of the Lie group, the proposed method allows for accurate solutions without the errors introduced by linearization. The incremental updates for pose and velocity in the DR computation are represented in the Lie algebra. Experimental results confirm that the proposed method improves the accuracy of DR. In particular, as the motion prediction time interval of the vehicle increases, the proposed method demonstrates a more pronounced improvement in positional accuracy.

1. Introduction

In the realm of pose (position and attitude) estimation, dead-reckoning (DR) plays a pivotal role, particularly in Kalman filter (KF)-based methods where it is utilized during the prediction stage. The accuracy of DR is paramount in improving the performance of pose estimation algorithms. As DR is employed to forecast the future state of a vehicle based on its current state and motion dynamics, any inaccuracies in DR computations can significantly impact the accuracy of the estimation process. Therefore, ensuring precise DR calculations is essential to enhance the overall estimation performance [1,2,3,4]. However, most previously used DR approaches are prone to errors due to inevitable approximations in numerical integration and approximations in attitude-related operations. In other words, they face issues such as decreasing accuracy of position with increasing time of numerical integration and problems in handling attitude and rotation operations in Cartesian space.

DR involves predicting the position and attitude of a vehicle through integration, which introduces approximation errors due to sensor-measured data integration over the motion prediction time [5]. These errors accumulate over time, leading to significant discrepancies between the predicted and actual poses [6,7]. Although shorter prediction time intervals may reduce errors, precision is not guaranteed. Consequently, implementing DR with approximate integration decreases prediction accuracy and increases uncertainty, especially in recursive filters like the KF. Additionally, as the prediction cycle of the vehicle increases, the method’s accuracy diminishes, making it unsuitable for position prediction with long velocity measurement cycles. Therefore, ensuring precise DR calculations is essential to enhance the overall prediction performance.

The rotation and attitude of a vehicle cannot be perfectly represented and computed within the linear space of Cartesian coordinates [8,9]. When the attitude is approximated and calculated within this linear space, errors due to approximation arise, leading to a decrease in position accuracy. Numerous researchers have assumed that attitudes, represented by Euler angles, form a linear space and have performed calculations based on this assumption. However, Euler angles can lead to gimbal lock, where a particular rotation axis becomes aligned with another axis [10,11]. Euler angles indeed encounter difficulties in accurately representing all attitudes, particularly when the pitch axis nears ±90 degrees, thereby complicating processing and representation tasks [12,13]. However, when it comes to DR, the challenges are more nuanced and stem from the limitations of Euler angles in expressing and operating rotations. Specifically, the use of Euler angles introduces singularities, such as gimbal lock, which can disrupt the continuity of orientation measurements and lead to inaccuracies in pose estimation during DR calculations. Furthermore, the nonlinear nature of Euler angle transformations complicates the integration process, making it susceptible to cumulative errors over time. These issues highlight the inadequacy of Euler angles in robustly representing motion dynamics, particularly in dynamic environments where precise pose estimation is crucial for navigation and localization tasks. The primary limitation of previously used DR methods stems from approximating the motion of the vehicle during a single sampling period as linear, leading to errors in position calculation. Another significant issue arises from the linear piecewise approximation of the continuous and coupled changes in pose, which introduces uncertainties. These uncertainties include errors resulting from simply integrating angular rates to estimate changes in attitude, the assumption of attitudes forming a linear space, and the occurrence of gimbal lock.

Research aimed at minimizing cumulative errors to enhance the accuracy of DR is actively being conducted across various fields [14,15,16,17,18]. Shurin et al. (2022) proposed pedestrian DR to address the drift issue over time caused by errors and noise in inertial sensor measurements in the navigation of quadcopters [14]. Jang et al. (2024) developed a position and direction estimation algorithm for active driving of wheelchairs using ultra-wideband (UWB) sensors [15]. Zhang et al. (2023) proposed an accurate underwater DR mathematical model based on the motion model of autonomous underwater vehicles (AUVs) to recursively compute ground truth and associated errors [16]. Bai et al. (2023) proposed a method for tracking upper limb segments using a single inertial sensor to reduce drift in DR-based position tracking [17]. Cao et al. (2023) proposed a DR method using only wheel speeds to achieve accurate path tracking in extreme situations where location sensors are absent [18].

To further enhance the robustness and accuracy of DR systems, recent studies have explored the integration of factor graphs [19,20]. Zhang et al. (2024) optimized AUV navigation by integrating side-scan sonar (SSS) data and inertial measurement units (IMU) using factor graphs, demonstrating significant improvements in positioning accuracy under challenging conditions [19]. Similarly, Ma et al. (2021) developed a multi-source information fusion algorithm based on factor graphs for AUVs, achieving enhanced computational efficiency and navigation performance by processing asynchronous heterogeneous measurements [20]. Additionally, information geometry has been employed to optimize navigation algorithms by enhancing their robustness and accuracy [21,22]. Shahoud et al. (2022) utilized street geometry information to improve visual navigation and path tracking for drones. This approach compensated for visual odometry drift through scene matching and servo control, leveraging known street geometries to enhance the precision of image alignment [21]. Moreover, information geometry principles have been applied to improve unmanned aerial vehicle (UAV) circular control by optimizing Fisher information, thus enhancing the stability and accuracy of the navigation system [22]. These advanced approaches address the limitations of traditional methods by effectively handling attitude and rotation in nonlinear spaces and reducing numerical approximation errors.

To overcome these limitations, this paper proposes a DR approach by representing the pose and velocity of the vehicle in the three-dimensional special Euclidean group, an extended Lie group (${\mathit{SE}}_2$(3)), based on Lie theory [23,24,25]. The proposed method resolves the approximation error issue of previously used DR by computing the attitude of objects moving in nonlinear spaces. Additionally, as the attitude of the vehicle is also represented by the Lie group, the 3D special orthogonal group (SO(3)), there is no need to approximate the attitude in a linear space, thus addressing the issue of model uncertainty concerning attitude. Lie theory-based DR handles the integration of the attitude of vehicles accurately and without approximation, significantly reducing the influence of time intervals compared to previously used DR methods [26,27]. Constructing the state as a Lie group in Lie theory-based DR provides highly accurate spatial representations, overcoming the limitations of previously used DR methods and achieving improved performance in terms of accuracy, consistency, and stability.

The main contributions and novel aspects of this study can be summarized as follows:

• Enhanced Accuracy in Pose Prediction: The proposed method significantly improves the accuracy of pose prediction for vehicle motion. By employing Lie theory-based methods, the approach enables rigorous DR calculations without approximations, precisely modeling translations and rotations, especially in three-dimensional space. This results in more accurate vehicle pose determination.
• Elimination of Linear Space Assumptions: Utilizing Lie groups, the method eliminates the necessity to assume that vehicle rotation or attitude forms a linear space. This avoids the need to force rotations or attitudes into a linear space, thereby preventing the introduction of approximation errors and enhancing overall prediction accuracy.
• Reduction of Cumulative Approximation Errors: The proposed method addresses and overcomes the limitations of conventional DR methods that suffer from cumulative approximation errors in integration operations. These errors are typically caused by inaccuracies in linear and angular velocity measurements, motion prediction time intervals, and computational limitations. By performing accurate computations in nonlinear spaces, the proposed method effectively reduces these cumulative errors, enhancing the overall estimation accuracy.
• Application in Pose Estimation Algorithms: The method can be seamlessly integrated as a process model in pose estimation algorithms, such as the KF. It provides highly accurate predictions of the current state during the prediction phase, thereby reducing estimation uncertainty and improving the robustness and reliability of the estimation process.

The significance of the results can be highlighted as follows:

• The results demonstrate a substantial improvement in the accuracy and reliability of vehicle pose prediction.
• The proposed method’s ability to mitigate cumulative errors and operate effectively in nonlinear spaces highlights its potential for practical applications in autonomous driving and other related fields.
• The findings suggest that adopting this method can lead to more precise and dependable navigation systems, ultimately contributing to the advancement of autonomous vehicle technologies.
• This novel approach not only enhances the theoretical foundation of vehicle pose estimation but also provides a practical solution that can be directly implemented in real-world autonomous systems.
• The method offers significant benefits in terms of performance and reliability, making it a valuable contribution to the field of autonomous driving.

In Section 2, we provide an overview of the previous DR methods commonly used and discuss their limitations. Section 3 presents an overview of the theoretical background, Lie theory, and describes the Lie theory-based DR method. Section 4 illustrates how the proposed method outperforms previously used methods using experimental results obtained from real-world data. Finally, in Section 5, we summarize the research findings and present future research directions.

2. Limitation of Previously Used Dead-Reckoning Approach

In this section, we describe the previously widely used DR approaches and discuss their limitations.

The state at time t consists of the position ${\mathbf{p}}_t$, velocity ${\dot{\mathbf{p}}}_t$, and attitude ${\mathit{\theta }}_t$ as follows:

$${\mathbf{X}}_t={\left[\begin{array}{ccc}{\mathbf{p}}_t& {\dot{\mathbf{p}}}_t& {\mathit{\theta }}_t\end{array}\right]}^T$$

(1)

In Equation (1), the position ${\mathbf{p}}_t$ and velocity ${\dot{\mathbf{p}}}_t$ of the vehicle belong to a three-dimensional real vector space. Both position and velocity are described in the east-north-up (ENU) coordinate frame.

$${\mathbf{p}}_t={\left[\begin{array}{ccc}{x}_t& y_t& z_t\end{array}\right]}^T.$$

(2)

$${\dot{\mathbf{p}}}_t={\left[\begin{array}{ccc}{\dot{x}}_t& {\dot{y}}_t& {\dot{z}}_t\end{array}\right]}^T.$$

(3)

The attitude ${\mathit{\theta }}_t$ is expressed as Euler angles as follows:

$${\mathit{\theta }}_t={\left[\begin{array}{ccc}{\varphi }_t& {\theta }_t& {\psi }_t\end{array}\right]}^T.$$

(4)

Previously widely used DR predicts the pose and velocity of a vehicle through a motion model from the state ${\mathbf{X}}_{t_k}$ and the measured input ${\mathit{U}}_{t_k}$ as follows:

$${\mathbf{X}}_{t_{k+1}}=f({\mathbf{X}}_{t_k},{\mathit{U}}_{t_k})+\mathbf{w}$$

(5)

where f(·) denotes a time-varying nonlinear function, and $\mathbf{w}$ is the process noise. The expression ${\mathit{U}}_{t_k}$ is the measured input and is expressed as follows:

$${\mathit{U}}_{t_k}=\left[\begin{array}{c}{\mathbf{v}}_{t_k}\\ {\mathbf{a}}_{t_k}\\ {\mathit{\omega }}_{t_k}\end{array}\right].$$

(6)

In Equation (6), ${\mathbf{v}}_{t_k}={\left[\begin{array}{ccc}{u}_{t_k}& v_{t_k}& w_{t_k}\end{array}\right]}^T$ is the linear velocity of the vehicle, ${\mathbf{a}}_{t_k}=$${\left[\begin{array}{ccc}{a_x}_{t_k}& {a_y}_{t_k}& {a_z}_{t_k}\end{array}\right]}^T$ is the acceleration of the vehicle, and ${\mathit{\omega }}_{t_k}={\left[\begin{array}{ccc}{p}_{t_k}& q_{t_k}& r_{t_k}\end{array}\right]}^T$ is the angular velocity of the vehicle.

The position, velocity, and attitude of the vehicle at the $k+1$th time step can be calculated as follows:

$${\mathbf{X}}_{t_{k+1}}={\mathbf{X}}_{t_k}+\delta {\mathbf{X}}_{t_k}$$

(7)

Equation (7) is rephrased as

$$\left[\begin{array}{c}{\mathbf{p}}_{t_{k+1}}\\ {\dot{\mathbf{p}}}_{t_{k+1}}\\ {\mathit{\theta }}_{t_{k+1}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{t_k}\\ {\dot{\mathbf{p}}}_{t_k}\\ {\mathit{\theta }}_{t_k}\end{array}\right]+\left[\begin{array}{c}\delta {\mathbf{p}}_{t_k}\\ \delta {\dot{\mathbf{p}}}_{t_k}\\ \delta {\mathit{\theta }}_{t_k}\end{array}\right]$$

(8)

The change in position, $\delta {\mathbf{p}}_{t_k}$, is calculated from the rotation matrix (${\mathit{R}}_{t_k}$), linear velocity (${\mathbf{v}}_{t_k}$), and the data reception period ($▵t$).

$$\delta {\mathbf{p}}_{t_k}={\mathit{R}}_{t_k}·{\mathbf{v}}_{t_k}·▵t$$

(9)

where $▵t$ is $t_{k+1}-t_k$. In Equation (9), the linear velocity is the linear velocity in the body frame of the vehicle, so the coordinate transformation is performed to the ENU coordinate frame through the rotation matrix. The rotation matrix is expressed from the attitude of the vehicle as follows:

$$\begin{array}{cc}\hfill {\mathit{R}}_{t_k}& ={\mathit{R}}_{t_k}(z,{\psi }_{t_k})·{\mathit{R}}_{t_k}(y,{\theta }_{t_k})·{\mathit{R}}_{t_k}(x,{\varphi }_{t_k})\hfill \\ \hfill & =\left[\begin{array}{ccc}{\mathit{R}}_{t_k}(1,1)& {\mathit{R}}_{t_k}(1,2)& {\mathit{R}}_{t_k}(1,3)\\ {\mathit{R}}_{t_k}(2,1)& {\mathit{R}}_{t_k}(2,2)& {\mathit{R}}_{t_k}(2,3)\\ {\mathit{R}}_{t_k}(3,1)& {\mathit{R}}_{t_k}(3,2)& {\mathit{R}}_{t_k}(3,3)\end{array}\right]\hfill \end{array}$$

(10)

where

$$\begin{array}{cc}\hfill & {\mathit{R}}_{t_k}(1,1)=cos\left({\theta }_{t_k}\right)·cos\left({\psi }_{t_k}\right)\hfill \\ \hfill & {\mathit{R}}_{t_k}(1,2)=sin\left({\varphi }_{t_k}\right)·sin\left({\theta }_{t_k}\right)·cos\left({\psi }_{t_k}\right)-cos\left({\varphi }_{t_k}\right)·sin\left({\psi }_{t_k}\right)\hfill \\ \hfill & {\mathit{R}}_{t_k}(1,3)=cos\left({\varphi }_{t_k}\right)·sin\left({\theta }_{t_k}\right)·cos\left({\psi }_{t_k}\right)+sin\left({\varphi }_{t_k}\right)·sin\left({\psi }_{t_k}\right)\hfill \\ \hfill & {\mathit{R}}_{t_k}(2,1)=cos\left({\theta }_{t_k}\right)·sin\left({\psi }_{t_k}\right)\hfill \\ \hfill & {\mathit{R}}_{t_k}(2,2)=sin\left({\varphi }_{t_k}\right)·sin\left({\theta }_{t_k}\right)·sin\left({\psi }_{t_k}\right)+cos\left({\varphi }_{t_k}\right)·cos\left({\psi }_{t_k}\right)\hfill \\ \hfill & {\mathit{R}}_{t_k}(2,3)=cos\left({\varphi }_{t_k}\right)·sin\left({\theta }_{t_k}\right)·sin\left({\psi }_{t_k}\right)-sin\left({\varphi }_{t_k}\right)·cos\left({\psi }_{t_k}\right)\hfill \\ \hfill & {\mathit{R}}_{t_k}(3,1)=-sin\left({\theta }_{t_k}\right)\hfill \\ \hfill & {\mathit{R}}_{t_k}(3,2)=sin\left({\varphi }_{t_k}\right)·cos\left({\theta }_{t_k}\right)\hfill \\ \hfill & {\mathit{R}}_{t_k}(3,3)=cos\left({\varphi }_{t_k}\right)·cos\left({\theta }_{t_k}\right)\hfill \end{array}$$

The change in velocity, $\delta {\dot{\mathbf{p}}}_{t_k}$, is calculated from the acceleration ${\mathbf{a}}_{t_k}$ and $▵t$. However, because the acceleration ${\mathbf{a}}_{t_k}$ is the acceleration in the body frame of the vehicle, the coordinate transformation to the ENU coordinate frame must be performed as follows:

$$\delta {\dot{\mathbf{p}}}_{t_k}={\mathit{R}}_{t_k}·{\mathbf{a}}_{t_k}·▵t$$

(11)

The attitude change vector $\delta {\mathit{\theta }}_{t_k}$ is calculated using the angular velocity vector ${\mathit{\omega }}_{t_k}$. This expresses the relationship between angular velocity and attitude, modeling the time variation of attitude. The connection between angular velocity and attitude change is represented by the derivative of the rotation matrix with respect to the Euler angles.

$$\delta {\mathit{\theta }}_{t_k}=\left[\begin{array}{ccc}1& sin\left({\varphi }_{t_k}\right)·tan\left({\theta }_{t_k}\right)& cos\left({\varphi }_{t_k}\right)·tan\left({\theta }_{t_k}\right)\\ 0& cos\left({\varphi }_{t_k}\right)& -sin\left({\varphi }_{t_k}\right)\\ 0& sin\left({\varphi }_{t_k}\right)/cos\left({\theta }_{t_k}\right)& cos\left({\varphi }_{t_k}\right)/cos\left({\theta }_{t_k}\right)\end{array}\right]·{\mathit{\omega }}_{t_k}·▵t$$

(12)

The previous method corresponds to a first-order approximation.

$$\begin{array}{cc}\hfill f(t+▵t)& =f\left(t\right)+\sum _{k=1}^{\infty }\frac{f^{\left(k\right)}\left(t\right)}{k!}·{(▵t)}^{\left(k\right)}\hfill \\ \hfill & =f\left(t\right)+f^{\prime }\left(t\right)·▵t+\frac{f^{″}\left(t\right)}{2}·{(▵t)}^2+\cdots \hfill \\ \hfill & \simeq f\left(t\right)+f^{\prime }\left(t\right)·▵t\hfill \end{array}$$

(13)

Here, Equations (9), (11) and (12) correspond to $f^{\prime }\left(t\right)·▵t$ for position, velocity, and attitude, respectively.

Previously widely used DR inevitably leads to errors between the measured discrete rotation values and the actual continuous rotation values due to approximate integration. This is because the approximate integration error in the time interval accumulates, causing a drift phenomenon. As the integration error accumulates, it becomes difficult to accurately calculate the attitude as the dynamic movement time of the vehicle increases, leading to a decrease in position prediction accuracy. Additionally, since the attitude is forcibly approximated and expressed in a linear space, errors due to approximation occur, leading to a decrease in accuracy for position.

3. Lie Theory-Based Dead-Reckoning Approach

In this section, we propose a Lie theory-based DR to overcome the limitations mentioned in Section 2. The measured input is the same as in Section 2.

3.1. Lie Theory Tools for ${\mathit{SE}}_2\left(3\right)$ Dead-Reckoning

To overcome the limitations of previously widely used DR methods, this paper introduces Lie theory. The introduced Lie theory includes theories related to physical roles according to the concepts of Lie groups and Lie algebras.

Lie groups are used to represent continuous transformations in DR systems. The position, velocity, and attitude of a vehicle are expressed as elements of a Lie group, and these elements are used to change over time and perform various transformations. Lie groups are also important tools for understanding the symmetry of DR systems, allowing for the study and analysis of transformations performed by the system. Lie groups are one of the key concepts in algebraic structure and differential geometry. Mathematically, it is a special set that is both a group and a smooth manifold. The concept of a smooth manifold is related to differential geometry, and the smoothness of a Lie group means that the operation between transformations is differentiable. Commonly used Lie groups in robotics include the n-dimensional special orthogonal group ($\mathit{SO}\left(n\right)$) [28], which represents rotation transformations, the n-dimensional special unitary group ($\mathit{SU}\left(n\right)$), which plays an important role in quantum mechanics, and the special Euclidean group ($\mathit{SE}\left(3\right)$), which represents rotations and translations in three-dimensional space [29,30,31]. In addition, there is ${\mathit{SE}}_n\left(3\right)$ extended from $\mathit{SE}\left(3\right)$ for rigid body transformations related to transformations or kinematic changes [26,32,33,34].

Lie algebra is an important mathematical structure connected to Lie groups. Whereas Lie groups are special sets that are algebraically smooth and at the same time smooth manifolds, Lie algebras are algebraic objects that represent transformations around the identity element of the corresponding group. Lie algebras are used to provide information about small changes in Lie groups. Therefore, small dynamic changes or errors of a vehicle can be expressed as elements of a Lie algebra. Lie algebras are mainly defined as the tangent space of a Lie group, that is, the tangent space near the identity element of the group. In particular, Lie algebras representing small transformations in Lie groups are mainly expressed as a set of skew-symmetric matrices. Therefore, they are expressed as a set of matrices that satisfy $A^T=-A$. The size of the matrix depends on the dimension of the Lie group.

Therefore, in Lie theory-based DR, Lie groups and Lie algebras act as important tools for modeling the pose and velocity of vehicles and understanding transformations. These concepts are essential in the design and improvement of DR algorithms and play an important role in improving accuracy and stability. Additionally, Lie theory-based DR performs calculations for rotation and translation of vehicles directly within the Lie group, dividing each rotation into small steps and minimizing approximation errors. In other words, because global optimization is performed through calculations within the Lie group, cumulative errors and approximation errors can be minimized, and accuracy can be maintained.

Figure 1 illustrates the correspondence between Lie groups and Lie algebras and demonstrates the role of the $\exp (·)$ function, a key concept within Lie theory. The $\exp (·)$ function enables the mapping of elements from the Lie algebra to elements of the Lie group. If $x\left(t_k\right)$ represents a point on the Lie group ($\mathcal{M}$), the velocity at this point ($\frac{\partial x\left(t_k\right)}{\partial t}$) belongs to the tangent space of the Lie group at that point. The structure of this tangent space is consistently maintained throughout the manifold due to its natural smoothness. Notably, the tangent space at the identity element of the group corresponds to the Lie algebra ($\mathfrak{m}$). Elements within the Lie algebra are represented as skew-symmetric matrices. These matrices can be expressed using the operator ^, which converts column vectors into matrices. The exponential map function $\exp (·)$ is employed to ensure a unique transformation from the Lie algebra to the Lie group matrix. This transformation allows for the mapping of elements from the Lie algebra to their corresponding elements in the Lie group.

Mathematically expressing the relationship between the Lie group and Lie algebra, let $\mathcal{M}$ be a Lie group and $\mathfrak{m}$ be the Lie algebra corresponding to the Lie group $\mathcal{M}$. The function $\exp \left({\mathit{\tau }}^{^}\right)$ maps the Lie algebra element ${\mathit{\tau }}^{^}\in \mathfrak{m}$ to the Lie group element $\chi \in \mathcal{M}$. The expression $\exp \left(\mathit{\tau }\right)$ is defined differently depending on the group [33]. If ${\mathit{\tau }}^{^}\in {\mathfrak{se}}_2\left(3\right)$, then $\exp \left({\mathit{\tau }}^{^}\right)$ is as follows:

$$\begin{array}{cc}\hfill & \exp \left({\mathit{\tau }}^{^}\right)=\left[\begin{array}{ccc}\exp \left({\mathit{\theta }}_{\times }\right)& \mathbf{V}\left(\mathit{\theta }\right)\mathit{\xi }& \mathbf{V}\left(\mathit{\theta }\right)\mathit{\rho }\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]\in {\mathit{SE}}_2\left(3\right),\mathit{\tau }=\left[\begin{array}{c}\mathit{\rho }\\ \mathit{\xi }\\ \mathit{\theta }\end{array}\right]\in {\mathbb{R}}^9.\hfill \end{array}$$

(14)

$\mathbf{V}\left(\mathit{\theta }\right)$ and $\exp \left({\mathit{\theta }}_{\times }\right)$ are calculated as

$$\begin{array}{cc}\hfill & \mathbf{V}\left(\mathit{\theta }\right)=\mathbf{I}+\frac{1-cos\left(\theta \right)}{\theta }{\left[\mathbf{u}\right]}_{\times }+\frac{\theta -sin\left(\theta \right)}{\theta }{\left[\mathbf{u}\right]}_{\times }^2,\hfill \\ \hfill & \exp \left({\mathit{\theta }}_{\times }\right)=\mathbf{I}+sin\left(\theta \right){\left[\mathbf{u}\right]}_{\times }+(1-cos\left(\theta \right)){\left[\mathbf{u}\right]}_{\times }^2,\hfill \end{array}$$

(15)

where $\mathbf{\theta }=\theta \mathbf{u},\parallel \mathbf{u}\parallel =1$, and ${\left[\begin{array}{c}·\end{array}\right]}_{\times }$ is an operator that transforms a vector into a skew-symmetric matrix.

3.2. Lie Theory Application for Three-Dimensional Dead-Reckoning of Vehicles

The ${\mathit{SE}}_2\left(3\right)$ group is a group extended from $\mathit{SE}\left(3\right)$ for rigid body transformations related to kinematic changes. The DR process in the Lie group ${\mathit{SE}}_2\left(3\right)$ is as follows:

$$\begin{array}{cc}\hfill {\mathbf{X}}_{t_{k+1}}& ={\mathbf{X}}_{t_k}\oplus ({\mathit{U}}_{t_k}·▵t)\hfill \\ \hfill & ={\mathbf{X}}_{t_k}·\exp ({{\mathit{U}}_{t_k}}^{^}·▵t).\hfill \end{array}$$

(16)

In Equation (16), ⊕ is an operator that combines Lie group elements and Lie algebra elements, and the result of the operation belongs to the Lie group. The Lie group ${\mathit{SE}}_2\left(3\right)$ is a transformation matrix that constitutes the position, velocity, and attitude of a vehicle in 3D space, and is expressed as follows:

$${\mathit{SE}}_2\left(3\right):={\mathbf{X}}_{t_k}=\left\{\left[\begin{array}{ccc}\mathbf{R}\left({\mathit{\theta }}_{t_k}\right)& {\dot{\mathbf{p}}}_{t_k}& {\mathbf{p}}_{t_k}\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]\mid \mathbf{R}\left({\mathit{\theta }}_{t_k}\right)\in SO\left(3\right),{\dot{\mathbf{p}}}_{t_k}\in {\mathbb{R}}^3,{\mathbf{p}}_{t_k}\in {\mathbb{R}}^3\right\}.$$

(17)

The expressions ${\mathbf{p}}_{t_k}$ and ${\dot{\mathbf{p}}}_{t_k}$ also represent the position and velocity of the vehicle in the ENU coordinate frame. The measured input ${\mathit{U}}_{t_k}$ is the same as Equation (6) in Section 2. The expression $\exp ({{\mathit{U}}_{t_k}}^{^}·▵t)$ is calculated from Equation (15). The comprehensive overview of the entire algorithm is presented as a block diagram, as shown in Figure 2.

4. Experiments and Results

This section demonstrates the improvements achieved by applying Lie theory through real-world experimental data. The proposed method is compared with the previously widely used DR. The comparison focuses on the accuracy of the prediction results when the sensor data reception cycle is varied. The prediction results refer to the calculated position, velocity, and attitude. Hereafter, the proposed method will be referred to as “proposed”, and “Conv-DR” will represent the previous method, i.e., the previously widely used linear approximation-based DR.

4.1. Experimental Environment

The proposed method was validated using the KITTI dataset, which is widely employed in autonomous driving research by scholars [35]. This dataset captures recordings of vehicle movements in and around Karlsruhe, Germany, incorporating various real-world factors such as noise, drift, and friction. It encompasses camera images, laser scans, precise GPS measurements, and IMU acceleration data collected via a combined GPS/IMU system. Figure 3 shows a map where the colors encode GPS signal quality. The red track is precisely recorded using real time kinematic (RTK)-GPS correction values, the blue track is without correction, and the black track indicates areas excluded from the dataset due to unusable GPS signals.

The dataset scenarios provided by [35] are diverse, ranging from highways in rural areas around the medium-sized city of Karlsruhe to urban scenes with many static and dynamic objects. The data used in this paper are from a residential area, characterized by densely packed housing and commercial buildings, narrow roads, numerous intersections, and roadside parking spaces. This data is calibrated, synchronized, and timestamped, and includes both the rectified and raw image sequences. The experiments were conducted during the day.

The vehicle used in the experiments is a VW Passat station wagon modified for autonomous driving research. It is equipped with LiDAR, GPS/IMU, and four video cameras. Our analysis utilized data from four distinct locations, as illustrated in Figure 3, with specifics outlined in

Table 1. Organizing experiment data.

Stat.	Data
	(1)	(2)	(3)	(4)
Driving time (s)	537.1492	483.2752	287.5341	470.5266
Total distance (m)	4207	5070	2207	3722
Maximum speed (m/s)	12.3288	14.8504	11.6550	12.8511
Minimum speed (m/s)	0.0218	2.4847	0.0065	0.0136
Starting Lat. (°N)	48°59′6.4	48°59′15.4	48°58′57.2	48°2′58.3
Starting Lon. (°E)	8°23′38.4	8°28′11.1	8°23′25.3	8°23′47.7

Stat.: Statistic. Lat.: Latitude. Lon.: Longitude.

.

4.2. Position Results

The error of the calculated position in the ENU coordinate frame was compared with the error of the previously used DR. The performance of the method is compared based on the distance error. The distance error indicates the distance between the calculated position ${\widehat{\mathbf{p}}}_t={\left[\begin{array}{ccc}{\widehat{x}}_t& {\widehat{y}}_t& {\widehat{z}}_t\end{array}\right]}^T$ and the reference position ${\mathbf{p}}_t={\left[\begin{array}{ccc}{x}_t& y_t& z_t\end{array}\right]}^T$ as shown in Equation (18).

$$d({\widehat{\mathbf{p}}}_t,{\mathbf{p}}_t)=\sqrt{{({\widehat{x}}_t-x_t)}^2+{({\widehat{y}}_t-y_t)}^2+{({\widehat{z}}_t-z_t)}^2}$$

(18)

Here, the reference position was obtained using RTK-GPS, which shows the highest accuracy.

Figure 4, Figure 5, Figure 6 and Figure 7 show the predicted position trajectories on the $xy$ plane for the experiments conducted in the four regions indicated in Figure 3. The pink graph represents the trajectory measured by RTK-GPS, which is used as the reference in this figure. The red graph indicates the results calculated using the previous method, and the green graph shows the results from the proposed method. In each figure, (a) represents the results when the data reception period is 0.1 s, (b) represents the results for a data reception period of 0.5 s, and (c) represents the results for a data reception period of 1 s.

Both methods perform only prediction without correction through external measurements, leading to inevitable shifts and rotations in the calculated trajectories, with calculation errors accumulating over time and travel distance. However, the proposed method consistently exhibits a smaller calculation error than the previously widely used DR method, even as the data reception period increases, ensuring that the trajectory does not significantly deviate from the reference trajectory.

Comparing the figures, it can be observed that the proposed method (green graph) consistently shows closer alignment to the reference trajectory (pink graph) compared to the previous method (red graph). This difference becomes more pronounced as the data reception period increases. As the data reception period increases from 0.1 s (a) to 1 s (c), both methods show increasing deviations from the reference trajectory, but the deviation in the proposed method is significantly smaller. Additionally, while the degree of deviation varies slightly across the four regions, the overall trend remains consistent, indicating the robustness of the proposed method across different environments. These observations highlight the advantages of the proposed method in reducing cumulative errors and maintaining trajectory accuracy over extended periods and varying data reception intervals.

In

Table 2. Analysis of the distance error in the ENU coordinate frame.

Stat	Ti.	Md.	Distance Error (m)
			(1)	(2)	(3)	(4)
Mn	0.1 (s)	Conv. DR	57.7120	26.1833	17.1059 (B)	67.9617 (W)
		Proposed	57.6346	26.1902	17.2910	58.1539
	0.5 (s)	Conv. DR	88.5603	82.2978	46.8934	255.4870 (W)
		Proposed	59.5332	37.0723	21.3908 (B)	95.7408
	1 (s)	Conv. DR	396.2126	355.2294	107.9019	433.2224 (W)
		Proposed	223.9200	76.5151	56.8564 (B)	146.5781
Std	0.1 (s)	Conv. DR	23.0549	12.2213	10.0197 (B)	29.9314 (W)
		Proposed	23.0563	12.9547	10.3032	19.1637
	0.5 (s)	Conv. DR	44.6968	71.2740	42.1647	194.0085 (W)
		Proposed	28.8588	27.1759	11.7212 (B)	60.7879
	1 (s)	Conv. DR	236.6208	245.9355	94.0146	305.5643 (W)
		Proposed	125.8985	50.2558	25.2438 (B)	78.2221
RMS	0.1 (s)	Conv. DR	62.1466	28.8951	19.8243 (B)	74.2609 (W)
		Proposed	62.0753	29.2190	20.1280	61.2301
	0.5 (s)	Conv. DR	99.2005	108.8710	63.0623	320.8004 (W)
		Proposed	66.1591	45.9662	24.3917 (B)	113.4084
	1 (s)	Conv. DR	461.4909	432.0558	143.1139	530.1427 (W)
		Proposed	256.8863	91.5435	62.2085 (B)	166.1441

Ti.: Time interval. Md.: Method. Mn: Mean of the error. Std: Standard deviation of the error. B: Best case. W: Worst case.

, we compare the mean error (Mn), standard deviation of the error (Std), and root mean square (RMS) of the distance errors in the ENU coordinate frame. The proposed method consistently shows smaller errors than the previously widely used DR in most experimental results. This is because the proposed method operates directly on the Lie group, exactly representing the pose and velocity of the vehicle without approximation.

To highlight the performance differences, we have marked the best (B) and worst (W) cases within each group defined by the data reception period and methods. Specifically, for each combination of data reception period (0.1 s, 0.5 s, 1 s) and methods, we identified the minimum and maximum values across the columns (1), (2), (3), and (4). The minimum values were marked as the best cases (B), and the maximum values were marked as the worst cases (W). This approach ensures a clear comparison of performance metrics across different configurations. Whereas the previous method performs well only when the data reception period is small, the results indicate that as the data reception period increases, the proposed method consistently outperforms the previous method.

Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14 and Figure 15 graphically compare the distance errors of position prediction based on Table 2. The path predicted by the proposed method is closer to the true path than that predicted by the previous method. Any trajectory error in the proposed method arises from errors in measured velocity and inherent computation. Unlike Equations (9), (11) and (12), the motion Equation (16) does not involve any approximation. Additionally, the proposed method can reduce cumulative errors, thereby improving accuracy even when the data reception period is long. These results demonstrate the overall effectiveness of the proposed method under various conditions.

4.3. Speed Results

The calculated velocity errors in the ENU coordinate frame were compared with the previously used DR errors. The reference velocity was obtained using the speed output from the GPS. Figure 16, Figure 17, Figure 18, Figure 19, Figure 20, Figure 21, Figure 22 and Figure 23 graphically compare the errors in velocity prediction based on

Table 3. Analysis of the speed error in the ENU coordinate frame.

Stat	Ti.	Md.	Speed Error (m/s)
			(1)	(2)	(3)	(4)
Mn	0.1 (s)	Conv. DR	28.7996	34.1915	26.7012	548.2722 (W)
		Proposed	20.7235 (B)	41.1633	24.3850	384.1330
	0.5 (s)	Conv. DR	265.8342	379.7410	412.9848	1845.6265 (W)
		Proposed	85.4411 (B)	108.0631	196.7227	1082.5576
	1 (s)	Conv. DR	2160.5948 (W)	1932.6978	656.9144	2047.2024
		Proposed	1716.4039	679.1729	101.4829 (B)	1394.4212
Std	0.1 (s)	Conv. DR	20.7475	28.2561	22.5549	367.4999 (W)
		Proposed	14.5014 (B)	41.6856	20.2872	219.8812
	0.5 (s)	Conv. DR	204.8562	306.3114	318.4042	1253.6116 (W)
		Proposed	89.4406	80.5858 (B)	116.8470	762.7432
	1 (s)	Conv. DR	1402.8605 (W)	1310.4113	542.7032	1234.7650
		Proposed	1191.4488	381.7053	54.9646 (B)	795.9201
RMS	0.1 (s)	Conv. DR	35.4947	44.3561	34.9525	660.0443 (W)
		Proposed	25.2934 (B)	58.5842	31.7206	442.6126
	0.5 (s)	Conv. DR	335.6097	487.8831	521.4764	2231.1162 (W)
		Proposed	123.6324 (B)	134.8025	228.8079	1324.2764
	1 (s)	Conv. DR	2576.0799 (W)	2335.0586	852.0935	2390.7493
		Proposed	2089.4001	779.0858	115.4118(B)	1605.5838

. Similar to the position results, the proposed method shows mostly smaller or similar results in mean error, standard deviation, and RMS values compared to the previously widely used DR. Additionally, the proposed method shows improved results in terms of velocity prediction accuracy as the data reception cycle becomes longer. Unlike the previously widely used DR, the proposed method performs accurate calculations in nonlinear space. However, the previously widely used DR considers attitude as a variable in linear space. Therefore, the previously widely used DR employs typical arithmetic operations. These operations lead to approximation errors.

4.4. Attitude Results

Figure 24, Figure 25, Figure 26, Figure 27, Figure 28, Figure 29, Figure 30 and Figure 31 graphically compare the errors in attitude based on

Table 4. Analysis of the attitude error.

Stat	Ti.	Md.	Di.	Attitude Error (deg)
				(1)	(2)	(3)	(4)
Mn	0.1 (s)	Conv. DR	$\varphi$	0.0763	0.3077	0.2194	0.3015
			$\theta$	−0.0602	0.0373 (B)	0.1218	−0.3586
			$\psi$	1.0869	1.2329	2.3185 (W)	−0.6320
		Proposed	$\varphi$	0.0481	0.3408	0.1801	0.2553
			$\theta$	−0.0574	0.0405	0.1014	−0.0608
			$\psi$	1.0800	1.2151	2.3165	−0.3437
	0.5 (s)	Conv. DR	$\varphi$	−0.8458	1.0514	2.3939	−3.9552
			$\theta$	−2.8671	2.4405	1.4226	3.3844
			$\psi$	−1.2849	4.4015	2.4816	16.2129 (W)
		Proposed	$\varphi$	0.0034 (B)	−0.4830	0.5417	0.5554
			$\theta$	−0.9671	−0.0283	−0.3320	−0.0539
			$\psi$	−1.5038	3.6899	2.7614	2.4823
	1 (s)	Conv. DR	$\varphi$	15.9446	12.3953	8.0376	2.7170
			$\theta$	4.6852	−1.0113	10.9729	5.5419
			$\psi$	−49.2481 (W)	21.3984	0.8525	−1.1596
		Proposed	$\varphi$	9.8586	3.6791	0.9710	1.4649
			$\theta$	2.8610	1.2904	0.3972 (B)	1.7685
			$\psi$	4.9715	5.9009	3.8896	3.4412
Std	0.1 (s)	Conv. DR	$\varphi$	0.6347	1.8583	1.2082	13.7041 (W)
			$\theta$	0.6135	2.7149	1.2460	12.0352
			$\psi$	1.1821	1.2830	2.2789	3.3131
		Proposed	$\varphi$	0.5288	2.0863	1.0988	7.9447
			$\theta$	0.5090 (B)	2.9105	1.1268	7.5067
			$\psi$	1.1851	1.2749	2.2821	3.2049
	0.5 (s)	Conv. DR	$\varphi$	7.2562	10.7870	15.0643	60.2891
			$\theta$	7.3873	14.7664	17.4287	60.4238 (W)
			$\psi$	3.4161	4.1048	3.1230 (B)	29.9377
		Proposed	$\varphi$	6.0787	5.3967	4.7974	27.9586
			$\theta$	7.4019	5.4767	6.6260	26.6421
			$\psi$	3.2928	3.8430	3.1803	4.3988
	1 (s)	Conv. DR	$\varphi$	60.7190	56.7711	37.9345	65.4590
			$\theta$	66.6400 (W)	60.9945	42.3434	62.8656
			$\psi$	35.2968	43.0970	7.9762	30.0545
		Proposed	$\varphi$	39.9334	13.7924	6.2721	28.4368
			$\theta$	41.3916	11.4413	6.4881	27.2554
			$\psi$	13.9436	7.4663	5.3514 (B)	9.9476
RMS	0.1 (s)	Conv. DR	$\varphi$	0.6393	1.8836	1.2279	13.7074 (W)
			$\theta$	0.6164	2.7152	1.2519	12.0405
			$\psi$	1.6058	1.7794	3.2510	3.3728
		Proposed	$\varphi$	0.5310	2.1140	1.1134	7.9488
			$\theta$	0.5122 (B)	2.9108	1.1314	7.5069
			$\psi$	1.6034	1.7612	3.2518	3.2233
	0.5 (s)	Conv. DR	$\varphi$	7.2562	10.7870	15.0643	60.2891
			$\theta$	7.3873	14.7664	17.4287	60.4238 (W)
			$\psi$	3.4161	4.1048	3.1230 (B)	29.9377
		Proposed	$\varphi$	6.0787	5.3967	4.7974	27.9586
			$\theta$	7.4019	5.4767	6.6260	26.6421
			$\psi$	3.2928	3.8430	3.1803	4.3988
	1 (s)	Conv. DR	$\varphi$	62.7776	58.1086	38.7767	65.5153
			$\theta$	66.8045 (W)	61.0029	43.7424	63.1094
			$\psi$	60.5907	48.1170	8.0216	30.0769
		Proposed	$\varphi$	41.1323	14.2747	6.3468 (B)	28.4746
			$\theta$	41.4904	11.5139	6.5002	27.3128
			$\psi$	14.8033	9.5166	6.6156	10.5260

Di.: Direction. ϕ: Roll, θ: Pitch, ψ: Yaw.

. It can be confirmed that the proposed method shows mostly smaller values in mean error, standard deviation, and RMS. Attitude and rotation cannot be perfectly represented and computed in linear space. The previously widely used DR inevitably includes errors that occur in the linearization process. However, the proposed method accurately represents attitude and handles operations in a nonlinear space called the Lie group. Therefore, it does not cause errors that force approximation into a linear space, and it was able to reduce the overall DR error.

5. Conclusions

This paper presents a novel approach to DR for vehicles. This method utilizes Lie theory by treating the position and velocity of the vehicle as elements of the Lie group ${\mathit{SE}}_2\left(3\right)$. Previously widely used DR methods have faced challenges due to approximation errors when handling nonlinear transformations and uncertainties. However, this method enables accurate solutions without linear approximation errors by treating the position and velocity of the vehicle as elements of a Lie group. In the DR operation process, the increments for pose and velocity are expressed in the Lie algebra, which is a linear space. Real-world experimental results have confirmed that this method improves accuracy. Notably, even when the time interval is long and increments increase, this method significantly reduces errors compared to previously used DR. In other words, the proposed method can overcome approximation errors due to integration and uncertainty errors caused by forced approximation of attitude into a linear space. In future work, the proposed method will be used as a process model in recursive filters such as the KF to improve the accuracy of state prediction. Additionally, the convergence performance of internal parameters such as measurement innovation and Kalman gain will be enhanced based on the improved state prediction values.