The Invariant Extended Kalman Filter as a Stable Observer

This paper deals with invariant extended Kalman filter (IEKF) which has a trajectory-independent property by defining the error state using the Lie group, unlike the conventional Kalman filter. Inertial navigation system (INS) is a widely used navigation solution providing 6 degrees of freedom motion. Since the performance of the INS tends to deteriorate with time, it requires another aiding sensor. In the fusion of the INS and such aiding sensor, the Kalman filter or its variants are mainly adopted. Specifically, it is well-known that an IEKF is effective when the initial attitude estimation error is large. There are two types of IEKFs, left IEKF (LIEKF) and right IEKF (RIEKF). It is known to be advantageous to adopt the LIEKF for the left-invariant measurements and right-IEKF for the right-invariant measurements. However, there have been few studies that have analyzed in detail the degradation of performance when it is used with the opposite IEKF. Therefore, this paper evaluates the performance of left-/right-IEKF according to the invariancy of a sensor measurement.

High-precision navigation solutions are a main requirement for autonomous vehicle (AV) applications. Global navigation satellite systems (GNSSs) are the prime source of navigation information for such applications. However, some places such as tunnels, underpasses, inside parking garages, and urban high-rise buildings suffer from GNSS signal degradation or unavailability. Therefore, another system is required to provide a continuous navigation solution, such as the inertial navigation system (INS). The vehicle's onboard inertial measuring unit (IMU) is the main INS input measurement source. However, the INS solution drifts over time due to IMU-associated errors and the mechanization process itself. Therefore, INS/GNSS integration is the proper solution for both systems' drawbacks. Traditionally, a linearized Kalman filter (LKF) such as the extended Kalman filter (EKF) is utilized as a navigation filter. The EKF deals only with the linearized errors and suppresses the higher orders using the Taylor expansion up to the first order. This paper introduces a loosely coupled INS/GNSS integration scheme using the invariant extended Kalman filter (IEKF). The IEKF state estimate is independent of the Jacobians that are derived in the EKF; instead, it uses the matrix Lie group. The proposed INS/GNSS integration using IEKF is applied to a real road trajectory for performance validation. The results show a significant enhancement when using the proposed system compared to the traditional INS/GNSS integrated system that uses EKF in both GNSS signal presence and blockage cases. The overall trajectory 2D-position RMS error reduced from 19.4 m to 3.3 m with 82.98% improvement and the 2D-position max error reduced from 73.9 m to 14.2 m with 80.78% improvement.

Three examples of the stability properties of the invariant extended Kalman filter

The construction and improvement of 5G can empower navigation and positioning. 5G/SINS integrated navigation can broaden application scenarios of the traditional GNSS/SINS integrated navigation. Extended Kalman Filter (EKF) has poor convergence when the initial error is large, resulting in poor positioning accuracy. Compared with EKF, the Invariant EKF (InEKF) constructs the system state on the matrix Lie group, and its dynamic equation can describe the motion characteristics of objects more naturally and essentially. At the same time, InEKF obtains state-independent Jacobians at any linearization point. Therefore, we propose a 5G/SINS integrated navigation system based on InEKF. Furthermore, the accuracy and convergence are compared with result of EKF. The simulation experiment is carried out from two dimensions of different accuracy levels of SINS and different initial errors. Experimental results show that the performance of InEKF is significantly better than that of EKF when the SINS accuracy level is high; When the noise is large, although the model no longer satisfies the group affine property, the performance of InEKF is still better than EKF while the advantage is not as obvious as that with small noise. In addition, the larger the initial error is, the better performance of InEKF has than EKF.

Improving Inertial Navigation System Alignment using a Proportional-Integral Left-Invariant Extended Kalman Filter: A Robust Approach Against Inertial Sensors Errors

This paper considers sliding window filtering in an invariant framework for estimation of attitude and rate gyro bias in a matrix Lie group formulation. The multiplicative extended Kalman filter (MEKF) and invariant extended Kalman filter (IEKF), variants of the extended Kalman filter well suited to estimation on matrix Lie groups, are discussed. The sliding window formulation of both the MEKF and IEKF is presented, leading to the sliding window filter (SWF), the invariant SWF (ISWF), and the imperfect ISWF for systems that are not group affine. Simulation results for an attitude and heading reference system with bias are presented, comparing the ISWF to the traditional SWF, MEKF, and IEKF.

This paper proposes a novel invariant extended Kalman filter (IEKF), a modified version of the extended Kalman filter (EKF), for state-of-charge (SOC) estimation of lithium-ion (Li-ion) battery cells. Unlike conventional EKF methods where the correction term used to update the state is linearly proportional to the output error, this paper employs the IEKF where the correction term is independent of the output error, resulting in a significant reduction in the estimation error and improving the estimation accuracy. In contrast to classic method like the EKF and more contemporary ones like the square root variant of the Cubature Kalman Filter (SCKF), the IEKF can successfully mimic the nonlinear dynamics and mitigate measurement noise stochasticity. Moreover, even if the measurement model fails to fully capture the cell’s dynamics, the IEKF will still sustain a reasonable performance. Hence, IEKF outperforms the conventional EKF, and even the SCKF, which can diverge if a mismatch between the SOC measurement model and the true SOC measurement occurs. The derivation of the proposed method followed by experimental verification using commercial Li-ion battery cells are presented.

Recent advances in the utilization of Lie Groups for robotic localization have led to dramatic increases in the accuracy of estimation and uncertainty characterization. One of the novel methods, the Invariant Extended Kalman Filter (InEKF) extends the Extended Kalman Filter (EKF) by leveraging the fact that some error dynamics defined on matrix Lie Groups satisfy a log-linear differential equation. Utilization of these observations result in linearization with minimal approximation error, no dependence on current state estimates, and excellent convergence and accuracy properties. In this letter we show that the primary sensors used for underwater localization, inertial measurement units (IMUs) and doppler velocity logs (DVLs) meet the requirements of the InEKF. Furthermore, we show that singleton measurements, such as depth, can also be used in the InEKF update with minor modifications, thus expanding the set of measurements usable in an InEKF. We compare convergence, accuracy and timing results of the InEKF to a quaternion-based EKF using a Monte Carlo simulation and show notable improvements in long-term localization and much faster convergence with negligible difference in computation time.

This paper derives a contact-aided inertial navigation observer for a 3D\nbipedal robot using the theory of invariant observer design. Aided inertial\nnavigation is fundamentally a nonlinear observer design problem; thus, current\nsolutions are based on approximations of the system dynamics, such as an\nExtended Kalman Filter (EKF), which uses a system's Jacobian linearization\nalong the current best estimate of its trajectory. On the basis of the theory\nof invariant observer design by Barrau and Bonnabel, and in particular, the\nInvariant EKF (InEKF), we show that the error dynamics of the point\ncontact-inertial system follows a log-linear autonomous differential equation;\nhence, the observable state variables can be rendered convergent with a domain\nof attraction that is independent of the system's trajectory. Due to the\nlog-linear form of the error dynamics, it is not necessary to perform a\nnonlinear observability analysis to show that when using an Inertial\nMeasurement Unit (IMU) and contact sensors, the absolute position of the robot\nand a rotation about the gravity vector (yaw) are unobservable. We further\naugment the state of the developed InEKF with IMU biases, as the online\nestimation of these parameters has a crucial impact on system performance. We\nevaluate the convergence of the proposed system with the commonly used\nquaternion-based EKF observer using a Monte-Carlo simulation. In addition, our\nexperimental evaluation using a Cassie-series bipedal robot shows that the\ncontact-aided InEKF provides better performance in comparison with the\nquaternion-based EKF as a result of exploiting symmetries present in the system\ndynamics.\n

Underwater vehicles heavily depend on the integration of inertial navigation with Doppler Velocity Log (DVL) for fusion-based localization. Given the constraints imposed by sensor costs, ensuring the optimization ability and robustness of fusion algorithms is of paramount importance. While filtering-based techniques such as Extended Kalman Filter (EKF) offer mature solutions to nonlinear problems, their reliance on linearization approximation may compromise final accuracy. Recently, Invariant EKF (IEKF) methods based on the concept of smooth manifolds have emerged to address this limitation. However, the optimization by matrix Lie groups must satisfy the “group affine” property to ensure state independence, which constrains the applicability of IEKF to high-precision positioning of underwater multi-sensor fusion. In this study, an alternative state-independent underwater fusion invariant filtering approach based on a two-frame group utilizing DVL, Inertial Measurement Unit (IMU), and Earth-Centered Earth-Fixed (ECEF) configuration is proposed. This methodology circumvents the necessity for group affine in the presence of biases. We account for inertial biases and DVL pole-arm effects, achieving convergence in an imperfect IEKF by either fixed observation or body observation information. Through simulations and real datasets that are time-synchronized, we demonstrate the effectiveness and robustness of the proposed algorithm.

Legged robots require knowledge of pose and velocity in order to maintain stability and execute walking paths. Current solutions either rely on vision data, which is susceptible to environmental and lighting conditions, or fusion of kinematic and contact data with measurements from an inertial measurement unit (IMU). In this work, we develop a contact-aided invariant extended Kalman filter (InEKF) using the theory of Lie groups and invariant observer design. This filter combines contact-inertial dynamics with forward kinematic corrections to estimate pose and velocity along with all current contact points. We show that the error dynamics follows a log-linear autonomous differential equation with several important consequences: (a) the observable state variables can be rendered convergent with a domain of attraction that is independent of the system’s trajectory; (b) unlike the standard EKF, neither the linearized error dynamics nor the linearized observation model depend on the current state estimate, which (c) leads to improved convergence properties and (d) a local observability matrix that is consistent with the underlying nonlinear system. Furthermore, we demonstrate how to include IMU biases, add/remove contacts, and formulate both world-centric and robo-centric versions. We compare the convergence of the proposed InEKF with the commonly used quaternion-based extended Kalman filter (EKF) through both simulations and experiments on a Cassie-series bipedal robot. Filter accuracy is analyzed using motion capture, while a LiDAR mapping experiment provides a practical use case. Overall, the developed contact-aided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in system.

Land vehicles need high-precision navigational systems in which multi-sensor integration may be provided. Moreover, land vehicles regularly use Global Navigation Satellite Systems (GNSS) to estimate their position. Unfortunately, several locations, such as tunnels and inside parking garages, where GNSS signals cannot be detected. Several types of research have been conducted to improve positioning information using multi-sensor integration. Then, the vehicle needs another system for finding its location in GNSS-denied conditions, such as Inertial Navigation System (INS). Despite the accuracy of INS in short-time period use, inertial navigation systems (INS) are liable to drifts of their positioning solution due to the inertial sensor errors that are inherent to them; therefore, this problem leads to errors accumulation over time then integration techniques are used to eliminate the resulting errors. Moreover, many filters are used in the process of integration, such as the Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), Particular Filter (PF) and Invariant Extended Kalman Filter (IEKF). Moreover, this work introduces the left-invariant extended Kalman filter (LIEKF) as a navigation filter for a loosely coupled integration to eliminate positioning errors. Furthermore, the LIEKF is based on the symmetry-preserving observer theory, which claims that the estimation error depends on the theory of a Lie group matrix, and the proposed system INS/GPS-based LIEKF converges to constant values, unlike the traditional INS/GPS. Moreover, the proposed system INS/GPS-based LIEKF depends on State-estimate-independent Jacobians, and the LIEKF is more efficient and has better performance due to results such as the 2D position RMS error due to the INS/GPS-based EKF is 19.43m. However, the 2D position RMS error due to the INS/GPS-based LIEKF is 3.32m with 83% improvement. Moreover, the 2D position errors were enhanced using the INS/GPS-based LIEKF system compared to the INS/GPS-based EKF system.

In this paper, we propose a cooperative localization method based on invariant extended Kalman filter (EKF) robust to initial heading error. The technology for estimating the position of agents (for example, pedestrians, mobile robots) without pre-installed infrastructure can be used for various tasks, such as position mobile robots in factories and position firefighters at disaster sites. The method of estimating the position based on the inertial-measurement units (IMU) mounted on the agent can estimate the position without additional sensors, but the accuracy is reduced due to the accumulation of errors for a long time. Cooperative localization method is a method of correcting the position of the agent estimated based on IMU using the relative distance information between agents. Cooperative localization methods are mainly based on EKF. There is a term related to the estimated attitude in the transition matrix of the EKF. If the estimated attitude error is large, the filter state is propagated incorrectly and the estimation performance is degraded. The proposed method solves this problem by using the invariant EKF so that the attitude error does not affect the transition matrix. The state of the invariant EKF is modeled as a Lie group, and the transition matrix derived from the Lie group becomes independent of the estimated state. That is, even if there is an attitude error, the state can be exactly propagated. Experimental results show that the proposed method is robust to attitude error.

The calibration accuracy of the Doppler Velocity Log (DVL) error directly affects the performance of the SINS/DVL integrated navigation system. Previous studies of DVL calibration have not dealt with DVL measurement outliers. Therefore, in this paper, a novel DVL calibration method is proposed to reduce the influence of outliers on calibration accuracy. Introducing the Lie group theory to the DVL calibration study for the first time, and taking advantage of the Special Orthogonal Group of order 3 [SO(3)] representation, a new DVL calibration model based SO(3) group with the ability to represent large misalignment angles is established. And an invariant extended Kalman filter (IEKF) design for the DVL calibration model is introduced. Then, a robust IEKF is proposed by combining the linear error propagation equation of the SO(3)-based DVL calibration model with the statistical similarity measure (SSM) theory. The proposed robust IEKF algorithm enriches the IEKF theory and has the ability to accurately estimate the state of the Lie group model in outlier environments. The simulation and lake trial are used to illustrate the effectiveness and superiority of the proposed DVL calibration method.

For the initial alignment of strapdown inertial navigation system (SINS) under the condition of large azimuth misalignment, a high-precision and fast initial alignment method is proposed. Firstly, in the Lie group space, the system matrix F and the measurement matrix H, which are relatively independent of the state estimate, are derived, and the invariant extended Kalman filter (INEKF) architecture. The INEKF not only eliminates the influence of the current state estimation error on the system matrix F and the measurement matrix H, but also suppresses the positive feedback and inconsistency problems that occur in the state update process of EKF. Then, the SINS initial alignment model is established based on INEKF. Finally, simulation results show that this method can effectively improve the speed and accuracy of the initial alignment, especially under the condition of a large misalignment angle, the alignment speed and accuracy are also greatly improved.