scispace - formally typeset
Search or ask a question

Showing papers on "Invariant extended Kalman filter published in 2021"


Journal ArticleDOI
TL;DR: The simulation results and semi-physical experiments show that the application of the proposed adaptive Kalman filter can guarantee a higher estimation accuracy of the state variables.

19 citations


Journal ArticleDOI
31 May 2021
TL;DR: In this article, the Invariant Extended Kalman Filter (InEKF) was proposed to extend the Extended KF by leveraging the fact that some error dynamics defined on matrix Lie Groups satisfy a log-linear differential equation.
Abstract: 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.

17 citations


Journal ArticleDOI
Joon-Ha Kim1, Seungwoo Hong1, Gwanghyeon Ji1, Seunghun Jeon1, Jemin Hwangbo1, Jun-Ho Oh1, Hae-Won Park1 
30 Jun 2021
TL;DR: In this article, a state estimation algorithm for the legged robot by defining the problem as a Maximum A Posteriori (MAP) estimation problem and solving the problem with the Gauss-Newton algorithm is presented.
Abstract: This letter presents a state estimation algorithm for the legged robot by defining the problem as a Maximum A Posteriori (MAP) estimation problem and solving the problem with the Gauss-Newton algorithm. Moreover, marginalization by the Schur Complement method is adopted to make a fixed size problem. Each component of the cost function and its Jacobian are derived utilizing the SO(3) manifold structure, while we reparameterize the state with nominal state and variation to make linear algebra and vector calculus applied properly. Furthermore, a slip rejection method is proposed to reduce the erroneous effect of fault modeling of kinematics models. The proposed algorithm is verified by comparison with the Invariant Extended Kalman Filter (IEKF) in real robot experiments on various environments.

14 citations


Proceedings ArticleDOI
30 May 2021
TL;DR: In this article, an invariant Extended Kalman Filter (InEKF) is implemented to fuse inertial and velocity measurements from a tracking camera and leg kinematic constraints.
Abstract: This paper proposes a state estimator for legged robots operating in slippery environments. An Invariant Extended Kalman Filter (InEKF) is implemented to fuse inertial and velocity measurements from a tracking camera and leg kinematic constraints. The misalignment between the camera and the robot-frame is also modeled thus enabling auto-calibration of camera pose. The leg kinematics based velocity measurement is formulated as a right-invariant observation. Nonlinear observability analysis shows that other than the rotation around the gravity vector and the absolute position, all states are observable except for some singular cases. Discrete observability analysis demonstrates that our filter is consistent with the underlying nonlinear system. An online noise parameter tuning method is developed to adapt to the highly time-varying camera measurement noise. The proposed method is experimentally validated on a Cassie bipedal robot walking over slippery terrain. A video for the experiment can be found at https://youtu.be/VIqJL0cUr7s.

13 citations


Journal ArticleDOI
TL;DR: A new visual/inertial integrated navigation method, named Sliding-Window Factor Graph optimised algorithm with Dynamic prior information (DSWFG), is proposed that can achieve superior accuracy and stability in almost all motion scenes.
Abstract: Visual–Inertial Navigation Systems (VINS) plays an important role in many navigation applications. In order to improve the performance of VINS, a new visual/inertial integrated navigation method, named Sliding-Window Factor Graph optimised algorithm with Dynamic prior information (DSWFG), is proposed. To bound computational complexity, the algorithm limits the scale of data operations through sliding windows, and constructs the states to be optimised in the window with factor graph; at the same time, the prior information for sliding windows is set dynamically to maintain interframe constraints and ensure the accuracy of the state estimation after optimisation. First, the dynamic model of vehicle and the observation equation of VINS are introduced. Next, as a contrast, an Invariant Extended Kalman Filter (InEKF) is constructed. Then, the DSWFG algorithm is described in detail. Finally, based on the test data, the comparison experiments of Extended Kalman Filter (EKF), InEKF and DSWFG algorithms in different motion scenes are presented. The results show that the new method can achieve superior accuracy and stability in almost all motion scenes.

5 citations


DOI
01 Jan 2021
TL;DR: 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 is still better than EkF while the advantage is not as obvious as that with small noise.
Abstract: 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.

4 citations


Proceedings ArticleDOI
30 May 2021
TL;DR: In this paper, the authors proposed to use RIEKF SLAM algorithm in active SLAM where both the predicted SLAM results for choosing control actions and the actual estimated SLAM result applying the selected control actions are computed using RIEkF algorithms.
Abstract: Right invariant extended Kalman filter (RIEKF) based simultaneous localization and mapping (SLAM) proposed recently has shown to be able to produce more consistent SLAM estimates as compared with traditional EKF based SLAM methods, including some improved EKF SLAM methods such as observability constrained-EKF (OC-EKF) SLAM. Latest results have demonstrated that its performance is very close to optimization based SLAM algorithms such as iSAM. In this paper, we propose to use RIEKF SLAM algorithm in active SLAM where both the predicted SLAM results for choosing control actions and the actual estimated SLAM results applying the selected control actions are computed using RIEKF algorithms. The advantages over traditional EKF based active SLAM are the more accurate and consistent predicted uncertainty estimates which result in robustness of the active SLAM algorithm. The advantages over optimization based active SLAM is the reduced computational cost. Simulation results are presented to validate the advantages of the proposed algorithm3.

4 citations


Posted Content
20 Mar 2021
TL;DR: The level set Kalman filter (LSKF) as mentioned in this paper is an extension of Kalman filtering for continuous-discrete systems with nonlinear state-space models, which assumes the probability distribution can be approximated as a Gaussian, and updates the Gaussian distribution through a time-update step and a measurement update step.
Abstract: We propose a new extension of Kalman filtering for continuous-discrete systems with nonlinear state-space models that we name as the level set Kalman filter (LSKF). The LSKF assumes the probability distribution can be approximated as a Gaussian, and updates the Gaussian distribution through a time-update step and a measurement-update step. The LSKF improves the time-update step when compared to existing methods, such as the continuous-discrete cubature Kalman filter (CD-CKF) by reformulating the underlying Fokker-Planck equation as an ordinary differential equation for the Gaussian, thereby avoiding expansion in time. Together with a carefully picked measurement-update method, numerical experiments show that the LSKF has a consistent performance improvement over CD-CKF for a range of parameters, while also simplifies the implementation, as no user-defined timestep subdivision between measurements is required, and the spatial derivatives of the drift function are not explicitly needed.

2 citations


Proceedings ArticleDOI
30 May 2021
TL;DR: In this article, the authors considered the use of two position receivers and an inertial measurement unit (IMU) to estimate the position, velocity, and attitude of a rigid body, collectively called extended pose.
Abstract: This paper considers the use of two position receivers and an inertial measurement unit (IMU) to estimate the position, velocity, and attitude of a rigid body, collectively called extended pose. The measurement model consisting of the position of one receiver and the relative position between the two receivers is left invariant, enabling the use of the invariant extended Kalman filter (IEKF) framework. The IEKF possesses various advantages over the standard multiplicative extended Kalman filter, such as state-estimate-independent Jacobians. Monte Carlo simulations demonstrate that the two-receiver IEKF approach yields improved estimates over a two-receiver multiplicative extended Kalman filter (MEKF) and a singlereceiver IEKF approach. An experiment further validates the proposed approach, confirming that the two-receiver IEKF has improved performance over the other filters considered.

2 citations


Proceedings ArticleDOI
12 Jul 2021
TL;DR: In this article, the states of stereo MSCKF to the matrix Lie group using IEKF was presented to represent the state and the noise similar to the real world.
Abstract: Rigid body motion can be modeled as the Lie group. For that reason, its uncertainty also has to be represented in the Lie group, and it is crucial for the proper localization of robots moving with high-speed. Also, the rigid body transformations between camera-IMU, multiple cameras that their errors can highly disturb robust state estimation are elements of the Lie group and can be calibrated while estimating the robot pose by composing them into the state of the visual-inertial odometry pipelines. Unfortunately, only a few researchers dealt with the state estimation problem on the matrix Lie group. In this paper, we present the states of stereo MSCKF to the matrix Lie group using IEKF to represent the state and the noise similar to the real world. Besides, our algorithm includes visual-inertial and camera extrinsic parameters as the matrix Lie group for online calibration and recovery from poor initial extrinsic parameters or an accident during the movement that can cause the physical skew. As results, we evaluate our algorithm and compare it with the original algorithm. Also, we demonstrate our online calibration results during the flight on the EuRoC MAV dataset.

2 citations


Posted Content
TL;DR: In this article, a deep learning-based contact estimator for legged robots is proposed, which bypasses the need for physical contact sensors and takes multi-modal proprioceptive sensory data from joint encoders, kinematics and an inertial measurement unit as input.
Abstract: This work reports on developing a deep learning-based contact estimator for legged robots that bypasses the need for physical contact sensors and takes multi-modal proprioceptive sensory data from joint encoders, kinematics, and an inertial measurement unit as input. Unlike vision-based state estimators, proprioceptive state estimators are agnostic to perceptually degraded situations such as dark or foggy scenes. For legged robots, reliable kinematics and contact data are necessary to develop a proprioceptive state estimator. While some robots are equipped with dedicated contact sensors or springs to detect contact, some robots do not have dedicated contact sensors, and the addition of such sensors is non-trivial without redesigning the hardware. The trained deep network can accurately estimate contacts on different terrains and robot gaits and is deployed along a contact-aided invariant extended Kalman filter to generate odometry trajectories. The filter performs comparably to a state-of-the-art visual SLAM system.

Proceedings ArticleDOI
30 May 2021
TL;DR: In this article, the Manifold Invariant Extended Kalman Filter (MANIFOLD-EKF) is proposed for better consistency and accuracy in state estimation on manifolds.
Abstract: This work presents the Manifold Invariant Extended Kalman Filter, a novel approach for better consistency and accuracy in state estimation on manifolds. The robustness of this filter allows for techniques with high noise potential like ultra-wideband localization to be used for a wider variety of applications like autonomous metal structure inspection. The filter is derived and its performance is evaluated by testing it on two different manifolds: a cylindrical one and a bivariate b-spline representation of a real vessel surface, showing its flexibility to being used on different types of surfaces. Its comparison with a standard EKF that uses virtual, noise-free measurements as manifold constraints proves that it outperforms standard approaches in consistency and accuracy. Further, an experiment using a real magnetic crawler robot on a curved metal surface with ultra-wideband localization shows that the proposed approach is viable in the real world application of autonomous metal structure inspection.

Posted Content
TL;DR: Li et al. as mentioned in this paper proposed a right invariant extended Kalman filter (RI-EKF) for object-level SLAM, which automatically maintains the correct unobservable subspace.
Abstract: With the recent advance of deep learning based object recognition and estimation, it is possible to consider object level SLAM where the pose of each object is estimated in the SLAM process. In this paper, based on a novel Lie group structure, a right invariant extended Kalman filter (RI-EKF) for object based SLAM is proposed. The observability analysis shows that the proposed algorithm automatically maintains the correct unobservable subspace, while standard EKF (Std-EKF) based SLAM algorithm does not. This results in a better consistency for the proposed algorithm comparing to Std-EKF. Finally, simulations and real world experiments validate not only the consistency and accuracy of the proposed algorithm, but also the practicability of the proposed RI-EKF for object based SLAM problem. The MATLAB code of the algorithm is made publicly available.

Posted Content
TL;DR: In this paper, an invariant Extended Kalman Filter (InEKF) was proposed for bipedal walking on a rocking treadmill to ensure the rapid error convergence and observable base yaw angle.
Abstract: Real-world applications of bipedal robot walking require accurate, real-time state estimation. State estimation for locomotion over dynamic rigid surfaces (DRS), such as elevators, ships, public transport vehicles, and aircraft, remains under-explored, although state estimator designs for stationary rigid surfaces have been extensively studied. Addressing DRS locomotion in state estimation is a challenging problem mainly due to the nonlinear, hybrid nature of walking dynamics, the nonstationary surface-foot contact points, and hardware imperfections (e.g., limited availability, noise, and drift of onboard sensors). Towards solving this problem, we introduce an Invariant Extended Kalman Filter (InEKF) whose process and measurement models explicitly consider the DRS movement and hybrid walking behaviors while respectively satisfying the group-affine condition and invariant form. Due to these attractive properties, the estimation error convergence of the filter is provably guaranteed for hybrid DRS locomotion. The measurement model of the filter also exploits the holonomic constraint associated with the support-foot and surface orientations, under which the robot's yaw angle in the world becomes observable in the presence of general DRS movement. Experimental results of bipedal walking on a rocking treadmill demonstrate the proposed filter ensures the rapid error convergence and observable base yaw angle.

Patent
15 Apr 2021
TL;DR: In this paper, a method for estimating the status of a target that is implemented in a mobile radar moving within a given space, the radar receiving a plurality of estimation signals related to the target is presented.
Abstract: A method for estimating the status of a target that is implemented in a mobile radar moving within a given space, the radar receiving a plurality of estimation signals related to the target. The method comprises one or more iterations, each associated with a given moment and comprising a step of applying an invariant extended Kalman filter, each iteration providing an estimation of the status of the target represented by status data comprising a set of status parameters and an associated covariance matrix. The status data is defined in relation to a reference point associated with the position of the mobile radar at the moment in question. Each current iteration comprises: - an application of a transformation operation to the reference point (401), which provides a transformed reference point, the transformed reference point being associated with the position of the mobile radar at the moment associated with the current iteration; - an updating (402) in the transformed reference point of the status data determined in the previous iteration; - an application of the invariant extended Kalman filter. The step of applying the invariant extended Kalman filter comprises the steps consisting in: - Determining an a priori estimated status of the target (403) defined in the transformed reference point using data comprising the status data determined in the previous iteration that was updated in the transformed reference point, - Determining an a posteriori status of the target (404) defined in the transformed reference point using data comprising the data from the a priori estimated status of the target and the data from at least one of the estimation signals.

Posted Content
TL;DR: In this paper, a learning-based contact estimator for legged robots that bypasses the need for physical sensors and takes multi-modal proprioceptive sensory data as input is developed.
Abstract: This work develops a learning-based contact estimator for legged robots that bypasses the need for physical sensors and takes multi-modal proprioceptive sensory data as input. Unlike vision-based state estimators, proprioceptive state estimators are agnostic to perceptually degraded situations such as dark or foggy scenes. While some robots are equipped with dedicated physical sensors to detect necessary contact data for state estimation, some robots do not have dedicated contact sensors, and the addition of such sensors is non-trivial without redesigning the hardware. The trained network can estimate contact events on different terrains. The experiments show that a contact-aided invariant extended Kalman filter can generate accurate odometry trajectories compared to a state-of-the-art visual SLAM system, enabling robust proprioceptive odometry.

Posted Content
TL;DR: In this paper, an invariant Extended Kalman Filter (InEKF) is implemented to fuse inertial and velocity measurements from a tracking camera and leg kinematic constraints.
Abstract: This paper proposes a state estimator for legged robots operating in slippery environments. An Invariant Extended Kalman Filter (InEKF) is implemented to fuse inertial and velocity measurements from a tracking camera and leg kinematic constraints. {\color{black}The misalignment between the camera and the robot-frame is also modeled thus enabling auto-calibration of camera pose.} The leg kinematics based velocity measurement is formulated as a right-invariant observation. Nonlinear observability analysis shows that other than the rotation around the gravity vector and the absolute position, all states are observable except for some singular cases. Discrete observability analysis demonstrates that our filter is consistent with the underlying nonlinear system. An online noise parameter tuning method is developed to adapt to the highly time-varying camera measurement noise. The proposed method is experimentally validated on a Cassie bipedal robot walking over slippery terrain. A video for the experiment can be found at this https URL.

Journal ArticleDOI
06 Aug 2021
TL;DR: In this paper, a Gaussian process is used to learn a data-driven relationship from UWB range and RSS inputs to orientation outputs, which is then combined with a gyroscope in an invariant extended Kalman filter.
Abstract: It is essential that a robot has the ability to determine its position and orientation to execute tasks autonomously. Heading estimation is especially challenging in indoor environments where magnetic distortions make magnetometer-based heading estimation difficult. Ultra-wideband (UWB) transceivers are common in indoor localization problems. This letter experimentally demonstrates how to use UWB range and received signal strength (RSS) measurements to estimate robot heading. The RSS of a UWB antenna varies with its orientation. As such, a Gaussian process (GP) is used to learn a data-driven relationship from UWB range and RSS inputs to orientation outputs. Combined with a gyroscope in an invariant extended Kalman filter, this realizes a heading estimation method that uses only UWB and gyroscope measurements.

Journal ArticleDOI
TL;DR: In this article, a Gaussian process is used to learn a data-driven relationship from UWB range and RSS inputs to orientation outputs, which is then combined with a gyroscope in an invariant extended Kalman filter.
Abstract: It is essential that a robot has the ability to determine its position and orientation to execute tasks autonomously. Heading estimation is especially challenging in indoor environments where magnetic distortions make magnetometer-based heading estimation difficult. Ultra-wideband (UWB) transceivers are common in indoor localization problems. This letter experimentally demonstrates how to use UWB range and received signal strength (RSS) measurements to estimate robot heading. The RSS of a UWB antenna varies with its orientation. As such, a Gaussian process (GP) is used to learn a data-driven relationship from UWB range and RSS inputs to orientation outputs. Combined with a gyroscope in an invariant extended Kalman filter, this realizes a heading estimation method that uses only UWB and gyroscope measurements.

Posted Content
TL;DR: In this paper, the authors considered the use of two position receivers and an inertial measurement unit (IMU) to estimate the position, velocity, and attitude of a rigid body, collectively called extended pose.
Abstract: This paper considers the use of two position receivers and an inertial measurement unit (IMU) to estimate the position, velocity, and attitude of a rigid body, collectively called extended pose. The measurement model consisting of the position of one receiver and the relative position between the two receivers is left invariant, enabling the use of the invariant extended Kalman filter (IEKF) framework. The IEKF possesses various advantages over the standard multiplicative extended Kalman filter, such as state-estimate-independent Jacobians. Monte Carlo simulations demonstrate that the two-receiver IEKF approach yields improved estimates over a two-receiver multiplicative extended Kalman filter (MEKF) and a single-receiver IEKF approach. An experiment further validates the proposed approach, confirming that the two-receiver IEKF has improved performance over the other filters considered.