Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Simultaneous State Estimation and Contact Detection for Legged Robots by Multiple-Model Kalman Filtering

Marcel Menner, Karl Berntorp

TL;DR

The paper addresses the challenge of simultaneous state estimation and contact detection for legged robots by modeling robot motion as a switched system whose modes correspond to different feet in contact. It introduces an interacting multiple-model Kalman filter (IMM-KF) that jointly infers the active contact mode and estimates the trunk state, using IMU data and motor measurements with mode-conditioned inputs and pseudo measurements. Validation in Gazebo and on a Unitree A1 demonstrates significant improvements in state accuracy (about 2.5x reduction in full-state RMSE) and reliable, fast contact detection (within ~20 ms) at 200 Hz, highlighting real-time feasibility and robustness to varying contact conditions. The approach reduces dependence on external sensors, providing a software-only, physics-based solution that leverages physical limits and contact probabilities to enhance estimation and control in unstructured environments.

Abstract

This paper proposes an algorithm for combined contact detection and state estimation for legged robots. The proposed algorithm models the robot's movement as a switched system, in which different modes relate to different feet being in contact with the ground. The key element in the proposed algorithm is an interacting multiple-model Kalman filter, which identifies the currently-active mode defining contacts, while estimating the state. The rationale for the proposed estimation framework is that contacts (and contact forces) impact the robot's state and vice versa. This paper presents validation studies with a quadruped using (i) the high-fidelity simulator Gazebo for a comparison with ground truth values and a baseline estimator, and (ii) hardware experiments with the Unitree A1 robot. The simulation study shows that the proposed algorithm outperforms the baseline estimator, which does not simultaneous detect contacts. The hardware experiments showcase the applicability of the proposed algorithm and highlights the ability to detect contacts.

Simultaneous State Estimation and Contact Detection for Legged Robots by Multiple-Model Kalman Filtering

TL;DR

The paper addresses the challenge of simultaneous state estimation and contact detection for legged robots by modeling robot motion as a switched system whose modes correspond to different feet in contact. It introduces an interacting multiple-model Kalman filter (IMM-KF) that jointly infers the active contact mode and estimates the trunk state, using IMU data and motor measurements with mode-conditioned inputs and pseudo measurements. Validation in Gazebo and on a Unitree A1 demonstrates significant improvements in state accuracy (about 2.5x reduction in full-state RMSE) and reliable, fast contact detection (within ~20 ms) at 200 Hz, highlighting real-time feasibility and robustness to varying contact conditions. The approach reduces dependence on external sensors, providing a software-only, physics-based solution that leverages physical limits and contact probabilities to enhance estimation and control in unstructured environments.

Abstract

This paper proposes an algorithm for combined contact detection and state estimation for legged robots. The proposed algorithm models the robot's movement as a switched system, in which different modes relate to different feet being in contact with the ground. The key element in the proposed algorithm is an interacting multiple-model Kalman filter, which identifies the currently-active mode defining contacts, while estimating the state. The rationale for the proposed estimation framework is that contacts (and contact forces) impact the robot's state and vice versa. This paper presents validation studies with a quadruped using (i) the high-fidelity simulator Gazebo for a comparison with ground truth values and a baseline estimator, and (ii) hardware experiments with the Unitree A1 robot. The simulation study shows that the proposed algorithm outperforms the baseline estimator, which does not simultaneous detect contacts. The hardware experiments showcase the applicability of the proposed algorithm and highlights the ability to detect contacts.
Paper Structure (29 sections, 32 equations, 7 figures, 1 table)

This paper contains 29 sections, 32 equations, 7 figures, 1 table.

Table of Contents

  1. Introduction
  2. Algorithm Realization and Contributions
  3. Related Work
  4. Preliminaries
  5. Notation
  6. Single-Model Kalman Filter
  7. Prediction step
  8. Update step
  9. Interacting Multiple-Model Kalman Filter
  10. Interaction step
  11. Filtering step
  12. Probability update step
  13. Combination step
  14. Mode-dependent Motion Model
  15. Model as Switched System
  16. ...and 14 more sections

Figures (7)

  • Figure 1: Controller architecture. For the Gazebo validations, the controller publishes ROS topics that the robot uses. For the hardware experiments, the controller communicates with the robot via Ethernet.
  • Figure 2: Screenshot of Gazebo simulation environment.
  • Figure 3: Gazebo simulation results. Top: Vertical position tracking. It shows the ground truth (green), the estimate provided by the proposed IMM-KF (light blue), and the baseline (dark red). Bottom: Contact likelihoods of the four feet.
  • Figure 4: Hardware experiments with the Unitree A1.
  • Figure 5: Hardware experiment results. Top: Contact likelihoods of the four feet. Bottom: Feet positions calculated using forward kinematics.
  • ...and 2 more figures

Theorems & Definitions (4)

  • Remark 1
  • Remark 2
  • Remark 3: Confidence in Pseudo Measurements
  • Remark 4: Alternative Formulation