Sensor Query Schedule and Sensor Noise Covariances for Accuracy-Constrained Trajectory Estimation

Abhishek Goudar, Angela P. Schoellig

TL;DR

This paper introduces a novel framework for determining optimal sensor query schedules and noise covariances to achieve a specific trajectory estimation accuracy for mobile robots. By formulating this problem as a semidefinite program, the authors enable robots to intelligently balance sensor performance with resource constraints, ensuring desired accuracy without over-sampling or using overly precise (and thus costly) sensors.

What Problem Does This Paper Address?

Estimating a mobile robot's trajectory accurately is crucial for many applications, from autonomous navigation to robotics research. This estimation relies on combining a robot's motion model with noisy sensor measurements. The accuracy of this estimation is directly influenced by two key sensor characteristics: how precise the sensor is (its noise covariance) and how frequently it provides data (its query schedule). Intuitively, more frequent and precise measurements lead to better accuracy. However, practical constraints like cost, power consumption, and communication bandwidth limit how accurate or frequent these measurements can be.

Existing approaches often focus on minimizing posterior state covariance, which can lead to inefficient sensor usage, or they are tied to specific estimation algorithms. This paper addresses the gap of explicitly determining sensor parameters (query schedule and noise covariance) to meet a desired estimation accuracy, rather than just minimizing error in general. This allows for a more resource-aware approach and also helps identify when a desired accuracy is simply unachievable with given system constraints.

Methods

The core of this paper's contribution lies in its novel optimization framework. The authors leverage the Posterior Cramér-Rao Bound (PCRB), which provides an estimator-agnostic lower bound on the estimation error. They derive a recursive form for the PCRB that is computationally tractable.

The key innovation is incorporating the desired estimation accuracy, denoted by $k_a$, as a constraint within this PCRB framework. This is achieved by formulating the problem as a semidefinite program (SDP). Specifically, they aim to:

  1. Estimate Sensor Query Schedule: Minimize the time between sensor queries (or maximize the query rate) for a given sensor noise covariance, such that the desired accuracy is met. This is formulated as maximizing $\Delta t_k$ (time between queries) subject to the accuracy constraint derived from the PCRB.
  2. Estimate Sensor Noise Covariance: Minimize the sensor noise covariance (or maximize its precision) for a given sensor query schedule, to achieve the desired accuracy. This is formulated as minimizing the trace of the inverse covariance matrix (an A-optimal design criterion) subject to the accuracy constraint.

These SDPs can be solved efficiently using off-the-shelf solvers. A significant advantage of this approach is its estimator-agnostic nature, relying on the PCRB rather than a specific filter like an Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF).

Datasets

The paper does not introduce new datasets. Instead, it uses:

  • Simulated data: Generated based on a continuous-time motion model (white noise on velocity or constant velocity) and simulated sensor models (position and range sensors).
  • Real experimental data: Collected from an omnidirectional mobile robot equipped with a motion capture system for ground truth and UWB anchors for range measurements.

Key Insights and Ideas

  • Accuracy-Constrained Optimization: The most significant insight is framing sensor parameter selection as an optimization problem with a desired accuracy as a hard constraint. This shifts the focus from simply minimizing error to achieving a specific performance target, which is more practical for real-world applications.
  • Estimator-Agnostic Approach: By using the PCRB, the method provides sensor schedules and covariances that are valid for a wide range of estimators, making it more generalizable than algorithm-specific scheduling.
  • Feasibility Analysis: The framework can explicitly identify scenarios where a desired estimation accuracy is infeasible given the system dynamics and sensor limitations. This is crucial for setting realistic performance expectations.
  • Intelligent Resource Allocation: The method allows for judicious use of sensor resources. For instance, if a lower sensor query rate is sufficient for the desired accuracy, the robot can conserve power or bandwidth, or allow other robots to share the communication channel.

Key Results

The paper demonstrates the effectiveness of its approach through simulations and real-world experiments:

  • Sensor Query Schedule: In simulations and real experiments, the proposed optimal sensor query schedules consistently achieved the desired Root Mean Squared Error (RMSE) for both position and range sensors. When a suboptimal, lower query rate (ms/3) was used, the RMSE was significantly higher, often exceeding the desired accuracy.
  • Sensor Covariance Estimation: Similarly, the estimated sensor covariances, when used with a given query schedule, resulted in RMSE values lower than the desired accuracy. Using higher, suboptimal covariances led to increased RMSE, validating the method's ability to find precise-enough sensor parameters.
  • Feasibility Identification: The authors show that for certain desired accuracy levels, the optimization problem becomes infeasible, indicating that the target accuracy cannot be met with the given system parameters. They also highlight cases where the recommended query rates exceed sensor capabilities.

Limitations and Future Work

  • Limitations:
    • The current formulation assumes additive white Gaussian noise for process and measurement noise.
  • The method is primarily demonstrated for linear systems or systems linearized around a trajectory. Extension to highly nonlinear systems or manifolds might require further development.
  • The paper focuses on specific sensor models (position and range) and motion models (WNOV, constant velocity).
  • Future Work:
    • Inclusion of diverse motion models and estimation of process covariances.
  • Extension to nonlinear manifolds (e.g., SE(d)).
  • Exploring other design criteria for sensor covariance estimation (e.g., D-optimal, E-optimal).
  • Investigating the impact of correlated noise.
  • Real-time implementation and adaptive scheduling.

Summary updated

Abstract

Trajectory estimation involves determining the trajectory of a mobile robot by combining prior knowledge about its dynamic model with noisy observations of its state obtained using sensors. The accuracy of such a procedure is dictated by the system model fidelity and the sensor parameters, such as the accuracy of the sensor (as represented by its noise covariance) and the rate at which it can generate observations, referred to as the sensor query schedule. Intuitively, high-rate measurements from accurate sensors lead to accurate trajectory estimation. However, cost and resource constraints limit the sensor accuracy and its measurement rate. Our work's novel contribution is the estimation of sensor schedules and sensor covariances necessary to achieve a specific estimation accuracy. Concretely, we focus on estimating: (i) the rate or schedule with which a sensor of known covariance must generate measurements to achieve specific estimation accuracy, and alternatively, (ii) the sensor covariance necessary to achieve specific estimation accuracy for a given sensor update rate. We formulate the problem of estimating these sensor parameters as semidefinite programs, which can be solved by off-the-shelf solvers. We validate our approach in simulation and real experiments by showing that the sensor schedules and the sensor covariances calculated using our proposed method achieve the desired trajectory estimation accuracy. Our method also identifies scenarios where certain estimation accuracy is unachievable with the given system and sensor characteristics.
BibTeX
@article{goudar2025sensor,
  author = {Abhishek Goudar and Angela P. Schoellig},
  title = {{Sensor Query Schedule and Sensor Noise Covariances for Accuracy-Constrained Trajectory Estimation}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2025},
  doi = {10.1109/lra.2025.3570948},
}
Figure