This paper introduces a novel proprioceptive state estimator for legged robots that combines model-based filters and deep neural networks. Recent studies have shown that neural networks such as multi-layer perceptron or recurrent neural networks can estimate the robot states, including contact probability and linear velocity. Inspired by this, we develop a state estimation framework that integrates a neural measurement network (NMN) with an invariant extended Kalman filter. We show that our framework improves estimation performance in various terrains. Existing studies that combine model-based filters and learning-based approaches typically use real-world data. However, our approach relies solely on simulation data, as it allows us to easily obtain extensive data. This difference leads to a gap between the learning and the inference domain, commonly referred to as a sim-to-real gap. We address this challenge by adapting existing learning techniques and regularization. To validate our proposed method, we conduct experiments using a quadruped robot on four types of terrain: \textit{flat}, \textit{debris}, \textit{soft}, and \textit{slippery}. We observe that our approach significantly reduces position drift compared to the existing model-based state estimator.
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 …
Legged Robot State Estimation Using Invariant Neural-Augmented Kalman Filter with a Neural Compensator
Mechatronics, Systems and Control Lab (MSC Lab)（机电系统与控制实验室）；Department of Mechanical Engineering（机械工程系）；Korea Advanced Institute of Science and Technology (KAIST)（韩国科学技术院）
University of Michigan（密歇根大学）；University of California, Berkeley（加州大学伯克利分校）
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…
Robust State Estimation for Legged Robots with Dual Beta Kalman Filter
Tsinghua University（清华大学）；Peking University（北京大学）；SunRising AI Ltd（旭日人工智能有限公司）；College of Engineering, Peking University（北京大学工学院）；School of Vehicle and Mobility, Tsinghua University（清华大学车辆与运载学院）；State Key Laboratory of Intelligent Green Vehicle and Mobility, Tsinghua University（清华大学智能绿色车辆与交通全国重点实验室）；College of Artificial Intelligence, Tsinghua University（清华大学人工智能学院）
College of Engineering, University of Michigan（密歇根大学工程学院）
This paper derives a contact-aided inertial navigation observer for a 3D bipedal robot using the theory of invariant observer design. Aided inertial navigation is fundamentally a nonlinear observer design problem; thus, current solutions are based on approximations of the system dynamics, such as an Extended Kalman Filter (EKF), which uses a system's Jacobian linearization along the current best estimate of its trajectory. On the basis of the theory of invariant observer design by Barrau and Bonnabel, and in particular, the Invariant EKF (InEKF), we show that the error dynamics of the point co…
Safety concerns during the operation of legged robots must be addressed to enable their widespread use. Machine learning-based control methods that use model-based constraints provide promising means to improve robot safety. This study presents a modular safety filter to improve the safety of a legged robot, i.e., reduce the chance of a fall. The prerequisite is the availability of a robot that is capable of locomotion, i.e., a nominal controller exists. During locomotion, terrain properties around the robot are estimated through machine learning which uses a minimal set of proprioceptive sign…
Robotic Systems Lab, ETH Zürich（苏黎世联邦理工学院机器人系统实验室）
We present a model predictive controller (MPC) that automatically discovers collision-free locomotion while simultaneously taking into account the system dynamics, friction constraints, and kinematic limitations. A relaxed barrier function is added to the optimization's cost function, leading to collision avoidance behavior without increasing the problem's computational complexity. Our holistic approach does not require any heuristics and enables legged robots to find whole-body motions in the presence of static and dynamic obstacles. We use a dynamically generated euclidean signed distance fi…
Self-assessment is a key to safety and robustness in automated driving. In order to design safer and more robust automated driving functions, the goal is to self-assess the performance of each module in a whole automated driving system. One crucial component in automated driving systems is the tracking of surrounding objects, where the Kalman filter is the most fundamental tracking algorithm. For Kalman filters, some classical online consistency measures exist for self-assessment, which are based on classical probability theory. However, these classical approaches lack the ability to measure t…
This study considers the object localization problem and proposes a novel multiparticle Kalman filter to solve it in complex and symmetric environments. Two well-known classes of filtering algorithms to solve the localization problem are Kalman filter-based methods and particle filter-based methods. We consider these classes, demonstrate their complementary properties, and propose a novel filtering algorithm that takes the best from two classes. We evaluate the multiparticle Kalman filter in symmetric and noisy environments. Such environments are especially challenging for both classes of clas…
University of Massachusetts Lowell（马萨诸塞大学洛厄尔分校）；Purdue University（普渡大学）；University of Rhode Island（罗德岛大学）
State estimation for legged locomotion over a dynamic rigid surface (DRS), which is a rigid surface moving in the world frame (e.g., ships, aircraft, and trains), remains an under-explored problem. This paper introduces an invariant extended Kalman filter that estimates the robot's pose and velocity during DRS locomotion by using common sensors of legged robots (e.g., inertial measurement units (IMU), joint encoders, and RDB-D camera). A key feature of the filter lies in that it explicitly addresses the nonstationary surface-foot contact point and the hybrid robot behaviors. Another key featur…
Adaptive Model-Base Control of Quadrupeds via Online System Identification using Kalman Filter
Robotics Innovation Center, German Research Center for Artificial Intelligence (DFKI GmbH)（机器人创新中心，德国人工智能研究中心（DFKI GmbH））；AG Robotik, University of Bremen（机器人技术研究所，不莱梅大学）；Dynamics Division, Department of Mechanics & Maritime Sciences, Chalmers University of Technology（动力学研究所，机械与海洋科学系，查尔姆斯理工大学）
Department of Navigation Engineering, Naval University of Engineering（海军工程大学导航工程系）
This note is devoted to deriving the measurement update of the geometric extended Kalman filter using the multiplicative extended Kalman filtering approach, resulting in the attitude estimator referred as geometric multiplicative extended Kalman filter. The equivalence of the derived geometric multiplicative extended Kalman filter and geometric extended Kalman filter is also demonstrated in this note.
Many state estimation and control algorithms require knowledge of how probability distributions propagate through dynamical systems. However, despite hybrid dynamical systems becoming increasingly important in many fields, there has been little work on utilizing the knowledge of how probability distributions map through hybrid transitions. Here, we make use of a propagation law that employs the saltation matrix (a first-order update to the sensitivity equation) to create the Salted Kalman Filter (SKF), a natural extension of the Kalman Filter and Extended Kalman Filter to hybrid dynamical syst…
Kalman filter is presumably one of the most important and extensively used filtering techniques in modern control systems. Yet, nearly all current variants of Kalman filters are formulated in the Euclidean space $\mathbb{R}^n$, while many real-world systems (e.g., robotic systems) are really evolving on manifolds. In this paper, we propose a method to develop Kalman filters for such on-manifold systems. Utilizing $\boxplus$, $\boxminus$ operations and further defining an oplus operation on the respective manifold, we propose a canonical representation of the on-manifold system. Such a canonica…
TIER IV, Inc.（TIER IV公司）；Massachusetts Institute of Technology（麻省理工学院）；NASA Jet Propulsion Lab, California Institute of Technology（加州理工学院NASA喷气推进实验室）；University of Michigan（密歇根大学）；The University of Tokyo（东京大学）
This paper develops a new nonlinear filter, called Moment-based Kalman Filter (MKF), using the exact moment propagation method. Existing state estimation methods use linearization techniques or sampling points to compute approximate values of moments. However, moment propagation of probability distributions of random variables through nonlinear process and measurement models play a key role in the development of state estimation and directly affects their performance. The proposed moment propagation procedure can compute exact moments for non-Gaussian as well as non-independent Gaussian random…
Korea Advanced Institute of Science and Technology（韩国科学技术院）
This paper proposes a simple, accurate and computationally efficient method to apply the ordinary unscented Kalman filter developed in Euclidean space to systems whose dynamics evolve on manifolds.We use the mathematical theory called stable embedding to make a variant of unscented Kalman filter that keeps state estimates in closeproximity to the manifold while exhibiting excellent estimation performance. We confirm the performance of our devised filter by applying it to the satellite system model and comparing the performance with other unscented Kalman filters devised specifically for system…
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-rece…
This article is an exhaustive revision of concepts and formulas related to quaternions and rotations in 3D space, and their proper use in estimation engines such as the error-state Kalman filter. The paper includes an in-depth study of the rotation group and its Lie structure, with formulations using both quaternions and rotation matrices. It makes special attention in the definition of rotation perturbations, derivatives and integrals. It provides numerous intuitions and geometrical interpretations to help the reader grasp the inner mechanisms of 3D rotation. The whole material is used to dev…
Invariant Extended Kalman Filter (IEKF) has been successfully applied in Visual-inertial Odometry (VIO) as an advanced achievement of Kalman filter, showing great potential in sensor fusion. In this paper, we propose partial IEKF (PIEKF), which only incorporates rotation-velocity state into the Lie group structure and apply it for Visual-Inertial-Wheel Odometry (VIWO) to improve positioning accuracy and consistency. Specifically, we derive the rotation-velocity measurement model, which combines wheel measurements with kinematic constraints. The model circumvents the wheel odometer's 3D integra…
The Kalman filter operates by storing a Gaussian description of the state estimate in the form of a mean and covariance. Instead of storing and manipulating the covariance matrix directly, a square-root Kalman filter only forms and updates a triangular matrix square root of the covariance matrix. The resulting algorithm is more numerically stable than a traditional Kalman filter, benefiting from double the working precision. This paper presents a formulation of the square root Kalman filter that leverages the QR decomposition to dramatically simplify the resulting algorithm.
In this paper, the spacecraft attitude estimation problem has been investigated making use of the concept of matrix Lie group. Through formulation of the attitude and gyroscope bias as elements of SE(3), the corresponding extended Kalman filter, termed as SE(3)-EKF, has been derived. It is shown that the resulting SE(3)-EKF is just the newly-derived geometric extended Kalman filter (GEKF) for spacecraft attitude estimation. This provides a new perspective on the GEKF besides the common frame errors definition. Moreover, the SE(3)-EKF with reference frame attitude error is also derived and the …
