skip to main content


Title: Contact-aided invariant extended Kalman filtering for robot state estimation
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.  more » « less
Award ID(s):
1808051
NSF-PAR ID:
10158784
Author(s) / Creator(s):
; ; ;
Date Published:
Journal Name:
The International Journal of Robotics Research
Volume:
39
Issue:
4
ISSN:
0278-3649
Page Range / eLocation ID:
402 to 430
Format(s):
Medium: X
Sponsoring Org:
National Science Foundation
More Like this
  1. This paper introduces a new invariant extended Kalman filter design that produces real-time state estimates and rapid error convergence for the estimation of the human body movement even in the presence of sensor misalignment and initial state estimation errors. The filter fuses the data returned by an inertial measurement unit (IMU) attached to the body (e.g., pelvis or chest) and a virtual measurement of zero stance-foot velocity (i.e., leg odometry). The key novelty of the proposed filter lies in that its process model meets the group affine property while the filter explicitly addresses the IMU placement error by formulating its stochastic process model as Brownian motions and incorporating the error in the leg odometry. Although the measurement model is imperfect (i.e., it does not possess an invariant observation form) and thus its linearization relies on the state estimate, experimental results demonstrate fast convergence of the proposed filter (within 0.2 seconds) during squatting motions even under significant IMU placement inaccuracy and initial estimation errors. 
    more » « less
  2. This work presents a multiplicative extended Kalman filter (MEKF) for estimating the relative state of a multirotor vehicle operating in a GPS-denied environment. The filter fuses data from an inertial measurement unit and altimeter with relative-pose updates from a keyframe-based visual odometry or laser scan-matching algorithm. Because the global position and heading states of the vehicle are unobservable in the absence of global measurements such as GPS, the filter in this article estimates the state with respect to a local frame that is colocated with the odometry keyframe. As a result, the odometry update provides nearly direct measurements of the relative vehicle pose, making those states observable. Recent publications have rigorously documented the theoretical advantages of such an observable parameterization, including improved consistency, accuracy, and system robustness, and have demonstrated the effectiveness of such an approach during prolonged multirotor flight tests. This article complements this prior work by providing a complete, self-contained, tutorial derivation of the relative MEKF, which has been thoroughly motivated but only briefly described to date. This article presents several improvements and extensions to the filter while clearly defining all quaternion conventions and properties used, including several new useful properties relating to error quaternions and their Euler-angle decomposition. Finally, this article derives the filter both for traditional dynamics defined with respect to an inertial frame, and for robocentric dynamics defined with respect to the vehicle’s body frame, and provides insights into the subtle differences that arise between the two formulations. 
    more » « less
  3. null (Ed.)
    Over the last few decades, Gyro-Free Inertial Measurement Units (GF-IMUs) have been extensively researched to overcome the limitations of gyroscopes. This research presents a Non-coplanar Accelerometer Array (NAA) for estimating angular velocity with non-specific geometric arrangement of four or more triaxial accelerometers with non-coplanarity constraint. The presented proof of non-coplanar spacial arrangement also provides insights into propagation of the sensor noise and construction of the noise covariance matrices. The system noise depends on the singular values of the relative displacement matrix (between the sensors). A dynamical system model with uncorrelated process and measurement noise is proposed where the accelerometer readings are used simultaneously as process and measurement inputs. The angular velocity is estimated using an Extended Kalman Filter (EKF) that discretizes and linearizes the continuous-discrete time dynamical system. The simulations are performed on a Cube-NAA (Cu-NAA) comprising four accelerometers placed at different vertices of a cube.They analyze the estimation error for static and dynamic movement as the distance between the accelerometers (four accelerometers in cube-orientation) is varied. Here, the system noise is observed to decrease inversely with the length of the cube edge as the arrangement is kept identical. Consequently, the simulation results indicate asymptotic decrease in the standard error of estimation with edge length. The experiments are conducted on a Cu-NAA with five reflective optical markers. The reflective markers are visually tracked using Vicon® to construct the ground truth angular velocity. This unique experimental setup, apart from providing three degrees of rotational freedom of movement, also allows for three degrees of spacial translation (linear acceleration of the Cu-NAA in space). The simulation and experimental results indicate better performance of the proposed EKF as compared to one with correlated process and measurement noises. 
    more » « less
  4. This paper presents a method of tracking multiple ground targets from an unmanned aerial vehicle (UAV) in a 3D reference frame. The tracking method uses a monocular camera and makes no assumptions on the shape of the terrain or the target motion. The UAV runs two cascaded estimators. The first is an Extended Kalman Filter (EKF), which is responsible for tracking the UAV’s state, such as position and velocity relative to a fixed frame. The second estimator is an EKF that is responsible for estimating a fixed number of landmarks within the camera’s field of view. Landmarks are parameterized by a quaternion associated with bearing from the camera’s optical axis and an inverse distance parameter. The bearing quaternion allows for a minimal representation of each landmark’s direction and distance, a filter with no singularities, and a fast update rate due to few trigonometric functions. Three methods for estimating the ground target positions are demonstrated: the first uses the landmark estimator directly on the targets, the second computes the target depth with a weighted average of converged landmark depths, and the third extends the target’s measured bearing vector to intersect a ground plane approximated from the landmark estimates. Simulation results show that the third target estimation method yields the most accurate results. 
    more » « less
  5. This paper considers the self-localization of a tethered drone without using a cable-tension force sensor in GPS-denied environments. The original problem is converted to a state-estimation problem, where the cable-tension force and the three-dimensional position of the drone with respect to a ground platform are estimated using an extended Kalman filter (EKF). The proposed approach uses the data reported by the onboard electric motors (i.e., the pulse width modulation (PWM) signals), accelerometers, gyroscopes, and altimeter, embedded in the commercial-of-the-shelf (COTS) inertial measurement units (IMU). A system-identification experiment was conducted to determine the model that computes the drone thrust force using the PWM signals. The proposed approach was compared with an existing work that assumes known cable-tension force. Simulation results show that the proposed approach produces estimates with less than 0.3-m errors when the actual cable-tension force is greater than 1 N. 
    more » « less