Extended Kalman Filter State Estimation for Autonomous Competition Robots

Ethan Kou, Acshi HaggenmillerarXiv 2023 · DOI · arXiv · OpenReview

TL;DR

This paper demonstrates how an Extended Kalman Filter (EKF) can effectively fuse data from odometry, a physics-based prediction model, and computer vision landmark detection to significantly improve the localization accuracy of an autonomous robot. The findings are particularly relevant for robotics competitions like FTC, where precise navigation is key to success.

What Problem Does This Paper Address?

Autonomous mobile robots, especially in competitive environments like the First Tech Challenge (FTC), rely heavily on accurate localization – knowing their precise position and orientation on the field. Traditional localization methods, such as odometry (tracking wheel rotations) and landmark detection (recognizing visual cues), each have limitations. Odometry provides frequent but error-prone velocity data that accumulates over time, while landmark detection offers infrequent but more accurate position fixes. The challenge is to effectively combine these disparate data sources to achieve a robust and accurate state estimate for the robot, even when one sensor type is unavailable.

Methods

The core of this paper is the application and evaluation of an Extended Kalman Filter (EKF). The EKF is a powerful algorithm for state estimation in systems that are not perfectly linear. Here's a breakdown of the approach:

  • System Model: The robot's state is defined by its position (x, y), orientation (θ), and body velocities (ẋ, ẏ, θ̇).
  • Data Sources:
    • Odometry: Provides frequent measurements of wheel encoder velocities (EL, ER, Ea), which are used to estimate the robot's velocity.
  • Physics Model (Prediction): A mathematical model predicts the robot's next state based on its current state and control inputs (wheel velocities). This model incorporates noise.
  • Computer Vision (Landmark Detection): Detects navigation images on the field walls to provide infrequent but accurate position (xm, ym, θm) measurements.
  • Extended Kalman Filter (EKF): The EKF fuses these data sources in a three-step process:
  • Prediction: The EKF uses the physics model to predict the robot's next state and its associated uncertainty (covariance).
  • Measurement Update: When sensor data (odometry or landmark detection) becomes available, the EKF compares it to the predicted measurement.
  • Estimation: The EKF combines the prediction and the measurement, weighting them based on their respective uncertainties, to produce an improved, more accurate state estimate.
  • Nonlinearity Handling: Since the robot's movement (especially rotation) involves trigonometric functions, the system is nonlinear. The EKF handles this by using Jacobians (matrices of partial derivatives) to linearize the system locally at each time step.
  • Simulation: The entire system was implemented and tested in a Python simulation environment.

Datasets

The paper does not use pre-existing datasets. Instead, it relies on a simulated environment designed to mimic the FTC competition field and robot dynamics. The simulation generates synthetic sensor data (odometry and landmark detection) and applies noise to these measurements and the system model. A specific "cycle" path was designed for testing, involving straight lines, turns, and curves to challenge the localization algorithms.

Key Insights and Ideas

  • Fusion is Key: The most significant insight is that combining multiple, even imperfect, data sources through an EKF yields substantially better localization accuracy than relying on any single source alone.
  • EKF's Adaptability: The EKF's ability to handle nonlinearities and dynamically adjust the weighting of sensor inputs based on their uncertainty (variance) makes it well-suited for real-world robotics where sensor noise and model inaccuracies are common.
  • Landmark Detection's Importance: While odometry provides continuous updates, landmark detection is crucial for correcting accumulated odometry errors and preventing significant drift in position estimates.
  • Trade-offs in EKF: The paper highlights that while the EKF improves accuracy, the linearization process (using Jacobians) means it's not perfectly optimal, especially in highly nonlinear scenarios or with large time steps.

Key Results

The paper quantifies the improvement in localization accuracy using Root Mean Squared Error (RMSE) for various state variables (position, velocity, and orientation).

  • Velocity Accuracy: Fusing odometry with the prediction model using an EKF resulted in the lowest velocity RMSE compared to using only prediction or only odometry. For example, the θ RMSE was reduced from 0.6698 (model only) to 0.2286 (model + odometry).
  • Position Accuracy: Combining all three data sources (odometry, model, and camera) yielded the lowest position RMSE. The (x, y) RMSE was significantly reduced from 2.8704 (model + odometry) to 1.2140 (model + odometry + camera).
  • Landmark Impact: When landmark detection was unavailable, position errors accumulated over time. However, upon re-acquiring a landmark, the EKF could correct these errors, demonstrating the value of visual cues.
  • Odometry Drift: The simulation showed that without landmark correction, odometry errors can accumulate significantly, even over a relatively short period (e.g., 20-inch error in 5 cycles).

Limitations and Future Work

  • Linearization Assumption: The EKF's reliance on linear approximations (Jacobians) means its accuracy can degrade when the system is highly nonlinear or when the time step (Δt) is large. This is noted as a significant limitation.
  • System Nonlinearity: While using body velocities helps, highly nonlinear system dynamics can still challenge the EKF's performance.
  • Future Work:
    • Implement the EKF on a physical robot with a more realistic physics model that includes factors like acceleration, friction, and motor voltages.
  • Explore machine learning to create more accurate physics models.
  • Investigate particle filters as an alternative state estimation algorithm, particularly for systems with non-Gaussian noise.

Summary updated

Abstract

Autonomous mobile robot competitions judge based on a robot's ability to quickly and accurately navigate the game field. This means accurate localization is crucial for creating an autonomous competition robot. Two common localization methods are odometry and computer vision landmark detection. Odometry provides frequent velocity measurements, while landmark detection provides infrequent position measurements. The state can also be predicted with a physics model. These three types of localization can be "fused" to create a more accurate state estimate using an Extended Kalman Filter (EKF). The EKF is a nonlinear full-state estimator that approximates the state estimate with the lowest covariance error when given the sensor measurements, the model prediction, and their variances. In this paper, we demonstrate the effectiveness of the EKF by implementing it on a 4-wheel mecanum-drive robot simulation. The position and velocity accuracy of fusing together various combinations of these three data sources are compared. We also discuss the assumptions and limitations of an EKF.
BibTeX
@misc{kou2023extended,
  author = {Ethan Kou and Acshi Haggenmiller},
  title = {{Extended Kalman Filter State Estimation for Autonomous Competition Robots}},
  year = {2023},
  eprint = {2310.04459},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2310.04459},
}
Figure