💻 computer science

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

This paper proposes a novel approach using semidefinite programming to determine the optimal sensor query schedules or noise covariances required to achieve a specific trajectory estimation accuracy, thereby addressing the trade-offs between estimation performance and resource constraints.

Original authors: Abhishek Goudar, Angela P. Schoellig

Published 2026-02-20

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Abhishek Goudar, Angela P. Schoellig

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to draw a map of a friend's walk through a foggy park. You know how fast they usually walk and which direction they tend to go (this is your prior knowledge). However, you also have a friend with a walkie-talkie who shouts out the friend's location every few seconds (this is your sensor).

The problem is:

The walkie-talkie isn't perfect; sometimes the signal is fuzzy or distorted (this is sensor noise).
The friend with the walkie-talkie gets tired or the battery dies if they shout too often (this is resource constraints).

If they shout too rarely, your map will be full of big gaps and guesses. If the signal is too fuzzy, your map will be wobbly and inaccurate.

This paper is about finding the "Goldilocks" zone. It asks: "How often do we need to shout, and how clear does the signal need to be, to draw a map that is accurate enough for our specific needs?"

Here is a breakdown of the paper's ideas using simple analogies:

1. The Two Main Questions

The authors wanted to solve two specific puzzles:

Puzzle A (The Schedule): "We have a walkie-talkie with a known level of fuzziness. How often should we ask for a location update to ensure our map is accurate enough?"
Puzzle B (The Quality): "We can only ask for updates every 2 seconds. How clear (or expensive) does our walkie-talkie need to be to still get an accurate map?"

2. The "Crystal Ball" vs. The "Ruler"

Usually, engineers try to make their maps as perfect as possible, regardless of the cost. They try to minimize all errors.

The Old Way: "Let's shout every millisecond and use the most expensive, perfect walkie-talkie!" (This wastes money and battery).
The New Way (This Paper): "We only need the map to be accurate within a 1-meter circle. Let's figure out the minimum effort required to stay inside that circle."

The authors use a mathematical tool called the Posterior Cramér-Rao Bound (PCRB). Think of this as a "Crystal Ball" that predicts the worst-case error of your map before you even start drawing it. It tells you, "If you shout this often with this kind of device, your map will be at least this good (or bad)."

3. The "Traffic Light" System

The paper turns this prediction into a traffic light system for robot designers:

Green Light: "Great! With this sensor and this schedule, you will hit your accuracy target."
Red Light: "Stop! Even with the best possible schedule, your current sensors are too fuzzy to hit that target. You need better hardware."
Yellow Light: "You are close, but you need to shout a little more often."

4. How They Did It (The Recipe)

The authors created a mathematical recipe (called a Semidefinite Program) that acts like a smart calculator.

You tell the calculator: "I need my map to be accurate within 10 centimeters."
You tell it: "Here is how my robot moves."
You tell it: "Here is how noisy my sensors are."
The Calculator Spits Out: "Okay, you need to update your sensor every 0.5 seconds." OR "You need to buy a sensor that is 3 times less noisy."

5. Real-World Testing

They didn't just do this on a computer; they tested it on a real robot in a lab with "Ultra-Wideband" (UWB) radios (which are like high-tech GPS).

The Test: They calculated the perfect update rate.
The Result: When they used the calculated rate, the robot's path was perfect. When they slowed it down (used a "suboptimal" rate), the robot's path went outside the safe zone, proving their math was right.
The Bonus: They also showed that if you ask for an impossible accuracy (like 1 millimeter) with cheap sensors, the calculator correctly says, "Nope, that's impossible," saving you from wasting time trying to fix the unfixable.

Summary

In short, this paper gives robot builders a smart calculator to stop guessing. Instead of buying the most expensive sensors or updating them too frequently (wasting money and battery), you can use this method to find the exact, cheapest, and most efficient way to get the accuracy you actually need. It's like knowing exactly how much fuel you need to drive to the store, rather than just filling the tank to the brim every time.

1. Problem Statement

Trajectory estimation for mobile robots relies on combining a dynamic system model with noisy sensor observations. The accuracy of this estimation is fundamentally limited by two factors:

System Model Fidelity: How well the mathematical model represents the robot's physics.
Sensor Parameters: Specifically, the sensor noise covariance (accuracy) and the sensor query schedule (measurement frequency/rate).

The Core Challenge:
In real-world applications, resources (power, bandwidth, compute, cost) constrain sensor accuracy and sampling rates. Traditionally, engineers select sensor parameters to minimize state uncertainty (e.g., minimizing posterior covariance). However, this approach does not guarantee that a specific desired accuracy level is met, nor does it identify if a desired accuracy is physically impossible given the system constraints.

The Objective:
The authors aim to invert the standard estimation problem. Instead of asking "What is the accuracy given these sensors?", they ask: "What sensor query schedule and/or sensor noise covariance are required to achieve a specific, pre-defined trajectory estimation accuracy (RMSE)?" Furthermore, the method should identify scenarios where the desired accuracy is unachievable.

2. Methodology

The proposed framework formulates the problem as a convex optimization task using the Posterior Cramér-Rao Bound (PCRB).

A. Theoretical Foundation: PCRB

The authors utilize the PCRB as an estimator-agnostic lower bound on the Mean Squared Error (MSE) of any unbiased estimator.

Why PCRB? Unlike specific filters (e.g., Kalman Filter), the PCRB provides a theoretical limit independent of the specific estimation algorithm used.
Recursive Formulation: To make the problem computationally tractable, the authors derive a recursive form for the PCRB (Proposition 1). This avoids inverting large Fisher Information Matrices (FIM) at every step. The recursion updates the information matrix ( $\check{J}_{x_{k+1}}$ ) based on the previous step's information and the current process/measurement models.

B. Constraint Formulation

The desired accuracy is defined by a scalar $k_a$ , representing the maximum allowable Root Mean Square Error (RMSE).

The condition that the estimation error must be within $k_a$ is translated into a constraint on the error correlation matrix: $\lambda_{\max}(\check{P}) \leq k_a^2$ .
Using the relationship between the error matrix and the information matrix ( $\check{P} \succeq \check{J}^{-1}$ ), this is converted into a constraint on the information matrix: $\check{J} \succeq k_a^{-2} I$ .
This inequality is reformulated using the Schur complement into a Linear Matrix Inequality (LMI), resulting in a convex constraint $S_k(\theta) \succeq 0$ .

C. Optimization Problems

The authors define two distinct optimization problems, both solvable as Semidefinite Programs (SDPs):

Sensor Query Schedule Estimation:
- Input: Known sensor noise covariance ( $R$ ).
- Goal: Find the minimum measurement frequency (or maximum time interval $\Delta t_k$ ) required to meet the accuracy $k_a$ .
- Formulation: Minimize the query rate (or maximize the interval) subject to the LMI constraint. The problem is convex in the inverse of the time interval ( $m_k = 1/\Delta t_k$ ).
Sensor Noise Covariance Estimation:
- Input: Known sensor query schedule (frequency).
- Goal: Find the minimum sensor noise covariance (maximum accuracy) required to meet $k_a$ .
- Formulation: Minimize the trace of the inverse covariance matrix (A-optimal design) subject to the LMI constraint. The problem is convex in the inverse covariance ( $R^{-1}$ ).

Key Feature: If the SDP solver returns an "infeasible" status, it mathematically proves that the desired accuracy $k_a$ cannot be achieved with the given system dynamics and sensor capabilities.

3. Key Contributions

Accuracy-Constrained Optimization: An estimator-agnostic framework that calculates sensor parameters specifically to meet a target accuracy, rather than just minimizing uncertainty.
Dual Capability: The method can solve for either the required sampling rate (given sensor quality) or the required sensor quality (given sampling rate).
Feasibility Analysis: The ability to explicitly identify scenarios where a desired accuracy is unachievable, providing a "certificate of infeasibility" via the SDP solver.
Convex Formulation: The derivation of a recursive PCRB constraint that allows the problem to be solved efficiently using off-the-shelf SDP solvers (e.g., MOSEK).

4. Experimental Results

The authors validated their approach through both simulations and real-world experiments using an omnidirectional mobile robot with UWB (Ultra-Wideband) and position sensors.

Simulation:
- Schedule Estimation: For various target accuracies ( $k_a$ ), the calculated optimal query rate ( $m_s$ ) consistently resulted in RMSE $\leq k_a$ . Using a suboptimal rate ( $m_s/3$ ) resulted in RMSE $> k_a$ .
- Covariance Estimation: For a fixed rate, the calculated optimal covariance ( $R$ ) achieved the target accuracy. Using a higher covariance (worse sensor, $3R$ ) failed to meet the target.
- Feasibility: The system correctly identified that extremely high accuracy (e.g., $k_a = 0.001$ m) was infeasible given the noise levels.
Real Experiments:
- Using a robot in a motion-capture arena with UWB anchors, the method calculated query rates for position and range sensors.
- Results: Trajectories estimated using the calculated optimal rates stayed within the desired accuracy envelope. Trajectories using lower rates breached the envelope.
- Operational Limits: The method successfully identified that achieving certain high-precision targets (e.g., 0.02 m with position sensors) would require query rates (340 Hz) exceeding the hardware capabilities of the specific sensors used, demonstrating its utility for system design limits.

5. Significance and Impact

Resource Efficiency: This method prevents the "over-engineering" of sensor systems. Instead of using the highest possible sensor rate (which wastes bandwidth and power), engineers can calculate the minimum necessary rate to meet a specific mission requirement.
System Design: It aids in the selection of hardware. If a desired accuracy requires a sensor covariance that does not exist or a rate that is too high, the system designer knows immediately that the current sensor suite is insufficient.
Generalizability: While demonstrated on trajectory estimation, the framework is applicable to any state estimation problem where the system dynamics and measurement models are known, provided the PCRB can be computed.
Practical Validation: The transition from simulation to real-world UWB experiments confirms that the theoretical bounds hold in the presence of real-world noise and non-idealities.

In summary, this paper provides a rigorous mathematical tool to bridge the gap between system requirements (desired accuracy) and hardware specifications (sensor rate and quality), ensuring efficient and feasible robot perception system design.