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

Abhishek Goudar; Angela P. Schoellig

cs.ROFeb 18, 2026

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

Authors: Abhishek Goudar, Angela P. Schoellig

Organizations: The authors are with the Learning Systems and Robotics Lab at the Technical University of Munich, Germany, and the University of Toronto Institute for Aerospace Studies, Canada. They are also affiliated with the University of Toronto Robotics Institute, the Munich Institute of Robotics and Machine Intelligence (MIRMI), and the Vector Institute for Artificial Intelligence.

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.

Figures & tables

Panel of Figure 1 — Fig. 1: Application of the proposed sensor query schedule calculation approach to trajectory estimation. For a desired trajectory estimation root mean square error (RMSE), we calculate a sensor query schedule using our proposed method (top right). The calculated query schedule is then used to generate measurements on our robot for use in the trajectory estimation pipeline (bottom right). Results from real experiments showing the estimated trajectory, using the proposed query rate $m_{s}$ (optimal rate) and a lower query rate $m_{s}/3$ (suboptimal rate), the ground-truth (gt) trajectory, and the desired accuracy envelope ( $k_{a}$ ) are shown (bottom left). The trajectory estimated with the proposed query rate is consistently within the accuracy envelope whereas the trajectory estimated using the lower query rate breaches the accuracy envelope on several instances, showing the validity of our approach.

Panel of Figure 2 — Fig. 2: The error correlation matrix, $\check{\boldsymbol{P}}_{\mathbf{x}_{t}}$ , viewed as the ellipse $\{\mathbf{x}\in\mathbb{R}^{2}\mid\mathbf{x}^{T}(\check{\boldsymbol{P}}_{\mathbf{x}_{t}})^{-1}\mathbf{x}\leq 1\}$ (green), represents the spread of errors and the square root of its maximum eigenvalue, $\lambda_{\max}$ , gives the maximum error along any principle axis. To achieve desired accuracy $k_{a}$ , the error ellipse must be within the accuracy bound ellipse $\{\mathbf{x}\in\mathbb{R}^{2}\mid\mathbf{x}^{T}({k_{a}^{2}\mathbf{I}})^{-1}\mathbf{x}\leq 1\}$ (blue) at each time step $t$ (gray dashed line).

Fig. 3: Trajectory estimation results from simulation using position sensors (left) and range sensors (right) with different sensor query rates. Noise values for the position and the range sensors are

\sigma_{p}=0.08\,

\mathrm{m}

and $\sigma_{r}=0.08\,$\mathrm{m}

, respectively. The desired RMSE for a given accuracy value,

k_{a}

, is indicated by the black line in each case. RMSE values obtained using the proposed optimal sensor query rate,

m_{s}

(optimal rate), are equal to or lower than the desired accuracy

k_{a}

(black line), whereas the lower sensor query rate,

m_{s}/3

(suboptimal rate), results in higher RMSE, indicating the validity of the proposed sensor query rate.

Fig. 4: Trajectory estimation results from simulation using position sensors (left) and range sensors (right) of proposed covariance,

\mathbf{R}

(optimal cov.), and higher covariance,

\tilde{\mathbf{R}}=3\,\mathbf{R}

(suboptimal cov.). The desired RMSE for a given accuracy value,

k_{a}

, is indicated by the black line in each case. Using sensors with the proposed covariances, the trajectory estimation RMSE is lower than the desired accuracy

k_{a}

in each case. However, using sensors with higher covariances results in RMSE higher than the required accuracy, indicating the validity of our approach.

Fig. 5: Trajectory estimation RMSE from real experiments with position sensors sampled at the proposed sensor query rate,

m_{s}

(optimal rate), and a lower sensor query rate,

m_{s}/3

(suboptimal rate). The desired RMSE for different accuracy values,

k_{a}

, is shown by the gray line. In all cases, the estimated sensor query rate yields RMSE equal to or lower than the desired accuracy

k_{a}

, whereas the lower sensor query rate results in higher RMSE.

Query	RMSE ( $\mathrm{m}$ )
frequency	$k_{a}=0.07\,$ \mathrm{m}$$	$k_{a}=0.08\,$ \mathrm{m}$$	$k_{a}=0.09\,$ \mathrm{m}$$	$k_{a}=0.1\,$ \mathrm{m}$$
$m_{s}$	0.055	0.054	0.067	0.092
${m_{s}}/{3}$	0.072	0.155	0.171	0.156

TABLE I: Trajectory estimation RMSE using range sensors from real experiments. The proposed sensor query rate,

m_{s}

, achieves RMSE values lower than

k_{a}

as desired, whereas the sensor query rate,

m_{s}/3

results in RMSE values larger than

k_{a}

Sensor	RMSE ( $\mathrm{m}$ )
covariance	$k_{a}=0.07\,$ \mathrm{m}$$	$k_{a}=0.08\,$ \mathrm{m}$$	$k_{a}=0.09\,$ \mathrm{m}$$	$k_{a}=0.1\,$ \mathrm{m}$$
$\mathbf{R}_{p}$	0.049	0.064	0.074	0.0942
$\tilde{\mathbf{R}}_{p}=3\mathbf{R}_{p}$	0.073	0.107	0.122	0.204

TABLE II: Trajectory estimation RMSE with position sensors from real experiments. The proposed sensor covariance

\mathbf{R}_{p}

achieves RMSE values lower than

k_{a}

as desired, whereas the sensor covariance

\tilde{\mathbf{R}}_{p}=3\mathbf{R}_{p}

results in RMSE values larger than

k_{a}

Robot State Estimation

Robot state estimation infers a robot’s position, motion, orientation, and internal configuration from noisy sensors and physical constraints.

See all 200 papers

ARC: Adaptive Robust Joint State and Covariance Estimation
Jun 18, 2026Alexandre Hadji-Thomas, Andrew Stirling, James R. ForbesCovariance Estimation Robot State Estimation
Online Covariance Estimation in Averaged SGD: Improved Batch-Mean Rates and Minimax Optimality via Trajectory Regression
Apr 12, 2026Yijin Ni, Xiaoming HuoPolyak-Ruppert Averaging Covariance Estimation
H. Milton Stewart School of Industrial and Systems Engineering, Georgia Institute of Technology
A Smart-Scheduled Hybrid (SSH) EKF-FGO State Estimation
Jun 14, 2026Eric Levy, Soosan BeheshtiFactor Graph Optimization Robot State Estimation
Electrical, Computer, and Biomedical Engineering Toronto Metropolitan University Toronto, ON, Canada

Jun 18, 2026J/K move · Enter open · S save

ARC: Adaptive Robust Joint State and Covariance Estimation

Alexandre Hadji-Thomas, Andrew Stirling, James R. Forbes

Covariance Estimation Robot State Estimation Robust Estimation

Open paper AskPDF arXiv

Sensor measurements are frequently corrupted by outliers and non-Gaussian noise. These imperfections in the sensor data can cause classical state estimators to generate biased and unreliable state and uncertainty estimates. Robust estimators reject or downweight outliers but do not perform measurement covariance estimation, whereas joint state and covariance estimators assume Gaussian residuals and fixed loss shape parameters. Integrating these two capabilities into a single framework is an opportunity to simultaneously estimate both state and covariance in the presence of outliers. This paper proposes a unified Block-Coordinate Descent framework that combines a norm-aware adaptive robust loss, an Iteratively Reweighted Least-Squares state update, and a Minimum Weighted Covariance Determinant covariance estimator, yielding a self-tuning joint state and covariance estimator. The framework is evaluated in a Monte-Carlo simulation and on real-world ultra-wideband localization experiments in cluttered non-line-of-sight environments. Results show that the proposed estimator consistently recovers the true inlier measurement covariance and matches or exceeds the state estimation accuracy of all baselines, without requiring any manual parameter tuning.

Abstract

Robot State Estimation

Explore similar work

ARC: Adaptive Robust Joint State and Covariance Estimation

Online Covariance Estimation in Averaged SGD: Improved Batch-Mean Rates and Minimax Optimality via Trajectory Regression

A Smart-Scheduled Hybrid (SSH) EKF-FGO State Estimation