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

.


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 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 (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 theorybased 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: 1. 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.2. 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.3. 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.4. 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: 1.The results demonstrate a substantial improvement in the accuracy and reliability of vehicle pose prediction.2. 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.3. 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.4.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. 5.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.

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 p t , velocity ṗt , and attitude θ t as follows: In Equation (1), the position p t and velocity ṗ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.
The attitude θ t is expressed as Euler angles as follows: Previously widely used DR predicts the pose and velocity of a vehicle through a motion model from the state X t k and the measured input U t k as follows: where f (•) denotes a time-varying nonlinear function, and w is the process noise.The expression U t k is the measured input and is expressed as follows: In Equation (6) is the acceleration of the vehicle, and ω t k = p t k q t k r t k T is the angular velocity of the vehicle.The position, velocity, and attitude of the vehicle at the k + 1th time step can be calculated as follows: Equation ( 7) is rephrased as The change in position, δp t k , is calculated from the rotation matrix (R t k ), linear velocity (v t k ), and the data reception period (△t).
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: where The change in velocity, δ ṗt k , is calculated from the acceleration a t k and △t.However, because the acceleration 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: The attitude change vector δθ t k is calculated using the angular velocity vector ω 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.
The previous method corresponds to a first-order approximation.
Here, Equations ( 9), ( 11) and ( 12) correspond to f ′ (t) • △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.

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.

Lie Theory Tools for SE 2 (3) 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 (SO(n)) [28], which represents rotation transformations, the n-dimensional special unitary group (SU(n)), which plays an important role in quantum mechanics, and the special Euclidean group (SE(3)), which represents rotations and translations in three-dimensional space [29][30][31].In addition, there is SE n (3) extended from SE(3) 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(t k ) represents a point on the Lie group (M), the velocity at this point ( ∂x(t k ) ∂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 (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 M be a Lie group and m be the Lie algebra corresponding to the Lie group M. The function exp(τ ˆ) maps the Lie algebra element τ ˆ∈ m to the Lie group element χ ∈ M. The expression exp(τ) is defined differently depending on the group [33].If τ ˆ∈ se 2 (3), then exp(τ ˆ) is as follows: V(θ) and exp(θ × ) are calculated as where θ = θu, ∥u∥ = 1, and • × is an operator that transforms a vector into a skew- symmetric matrix.

Lie Theory Application for Three-Dimensional Dead-Reckoning of Vehicles
The SE 2 (3) group is a group extended from SE(3) for rigid body transformations related to kinematic changes.The DR process in the Lie group SE 2 (3) is as follows: 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 △t means t k+1 − t k .The Lie group SE 2 (3) is a transformation matrix that constitutes the position, velocity, and attitude of a vehicle in 3D space, and is expressed as follows: The expressions p t k and ṗt k also represent the position and velocity of the vehicle in the ENU coordinate frame.The measured input U t k is the same as Equation ( 6) in Section 2. The expression exp(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.

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.

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, and numerous intersections and roadside parking spaces.This data are calibrated, synchronized, and timestamped, and we provide 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.

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 p(t) = x(t) ŷ(t) ẑ(t) T and the reference position p(t) = x(t) y(t) z(t) T as shown in Equation (18).
Here, the reference position was obtained using RTK-GPS, which shows the highest accuracy.
Figures 4-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, 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.
Figures 8-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.

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.Figures 16-23 graphically compare the errors in velocity prediction based on Table 3.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.

Attitude Results
Figures 24-31 graphically compare the errors in attitude based on Table 4.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.

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 SE 2 (3).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 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.

Figure 2 .
Figure 2. Overview of the Lie theory-based dead-reckoning algorithm.

Figure 4 .Figure 5 .Figure 6 .Figure 7 .
Figure 4. Dead-reckoned trajectory calculated by the local data at region (1): (a) has a data reception period of 0.1 s; (b) has a data reception period of 0.5 s; and (c) has a data reception period of 1 s.

Figure 8 .
Figure 8.Comparison of distance error, mean, and standard deviation in (1): (a) has a data reception period of 0.1 s; (b) has a data reception period of 0.5 s; and (c) has a data reception period of 1 s.

Figure 9 .Figure 10 .Figure 11 .Figure 12 .Figure 13 .Figure 14 .Figure 15 .
Figure 9.Comparison of distance error, mean, and standard deviation in (2): (a) has a data reception period of 0.1 s; (b) has a data reception period of 0.5 s; and (c) has a data reception period of 1 s.

Figure 16 .Figure 17 .Figure 18 .Figure 19 .Figure 20 .Figure 21 .Figure 22 .Figure 23 .
Figure 16.Comparison of speed error, mean, and standard deviation in (1): (a) has a data reception period of 0.1 s; (b) has a data reception period of 0.5 s; and (c) has a data reception period of 1 s.
, v t k = u t k v t k w t k T is the linear velocity of the vehicle, a t k =

Table 2 .
Analysis of the distance error in the ENU coordinate frame.
Ti.: Time interval.Md.: Method.Mn: Mean of the error.Std: Standard deviation of the error.B: Best case.W: Worst case.

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

Table 4 .
Analysis of the attitude error.