Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

A Group Theoretic Metric for Robot State Estimation Leveraging Chebyshev Interpolation

Varun Agrawal, Frank Dellaert

TL;DR

This work proposes a new metric for robot state estimation based on the recently introduced SE2(3) Lie group definition that explicitly takes into account the linear velocity of the state estimate, improving over current pose-based trajectory analysis.

Abstract

We propose a new metric for robot state estimation based on the recently introduced $\text{SE}_2(3)$ Lie group definition. Our metric is related to prior metrics for SLAM but explicitly takes into account the linear velocity of the state estimate, improving over current pose-based trajectory analysis. This has the benefit of providing a single, quantitative metric to evaluate state estimation algorithms against, while being compatible with existing tools and libraries. Since ground truth data generally consists of pose data from motion capture systems, we also propose an approach to compute the ground truth linear velocity based on polynomial interpolation. Using Chebyshev interpolation and a pseudospectral parameterization, we can accurately estimate the ground truth linear velocity of the trajectory in an optimal fashion with best approximation error. We demonstrate how this approach performs on multiple robotic platforms where accurate state estimation is vital, and compare it to alternative approaches such as finite differences. The pseudospectral parameterization also provides a means of trajectory data compression as an additional benefit. Experimental results show our method provides a valid and accurate means of comparing state estimation systems, which is also easy to interpret and report.

A Group Theoretic Metric for Robot State Estimation Leveraging Chebyshev Interpolation

TL;DR

This work proposes a new metric for robot state estimation based on the recently introduced SE2(3) Lie group definition that explicitly takes into account the linear velocity of the state estimate, improving over current pose-based trajectory analysis.

Abstract

We propose a new metric for robot state estimation based on the recently introduced Lie group definition. Our metric is related to prior metrics for SLAM but explicitly takes into account the linear velocity of the state estimate, improving over current pose-based trajectory analysis. This has the benefit of providing a single, quantitative metric to evaluate state estimation algorithms against, while being compatible with existing tools and libraries. Since ground truth data generally consists of pose data from motion capture systems, we also propose an approach to compute the ground truth linear velocity based on polynomial interpolation. Using Chebyshev interpolation and a pseudospectral parameterization, we can accurately estimate the ground truth linear velocity of the trajectory in an optimal fashion with best approximation error. We demonstrate how this approach performs on multiple robotic platforms where accurate state estimation is vital, and compare it to alternative approaches such as finite differences. The pseudospectral parameterization also provides a means of trajectory data compression as an additional benefit. Experimental results show our method provides a valid and accurate means of comparing state estimation systems, which is also easy to interpret and report.
Paper Structure (21 sections, 1 theorem, 16 equations, 4 figures, 2 tables)

This paper contains 21 sections, 1 theorem, 16 equations, 4 figures, 2 tables.

Table of Contents

  1. INTRODUCTION
  2. RELATED WORK
  3. Preliminaries
  4. Lie Group and Lie Algebra
  5. Trajectory Evaluation
  6. STATE ESTIMATION METRIC
  7. State Definition
  8. $\mathrm{SE}_2(3)\xspace$ Group
  9. Alignment
  10. Absolute State Error
  11. Chebyshev Polynomial Interpolation
  12. Chebyshev Series & Polynomials
  13. Barycentric Interpolation
  14. Differentiation via Interpolation
  15. Fitting to Noisy Data
  16. ...and 6 more sections

Key Result

Theorem 1

Let $f$ be a continuous function on $\left[-1,1\right]$, and let $\epsilon>0$ be arbitrary. Then there exists a polynomial $p$ such that

Figures (4)

  • Figure 1: The ability to compare state estimation techniques with a singular value can aid in various design choices of a robot controller, especially those highly dependent on good state estimates, e.g. for dynamic walking. Here two state estimate trajectories are visualized for qualitative comparison.
  • Figure 2: Chebyshev points $\cos(k\pi/n)$ for degree $n=16.$ They are obtained by projecting a regular grid on the unit circle onto the x-axis.
  • Figure 3: Chebyshev polynomial fit to the trajectory collected from the CARLA simulator. The ground truth translation and linear velocities are shown as red lines, and the fitted translation and computed velocity are shown as green dots. Result from (centered) finite differences is also shown for comparison. The degree of the polynomial was arbitrarily chosen as 400.
  • Figure 5: Chebyshev polynomial fit to the translation and velocity data of an A1 quadruped walking in a diagonal left-front trajectory. The ground truth data is collected in the Pybullet simulation environment Coumans16report.

Theorems & Definitions (1)

  • Theorem 1: Weierstrass Approximation Theorem