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

Jun 18, 2026

ARC: Adaptive Robust Joint State and Covariance Estimation

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.

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

Covariance Estimation Robot State Estimation Robust Estimation

Ask PDF

Apr 12, 2026

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

We study online covariance matrix estimation for Polyak--Ruppert averaged stochastic gradient descent (SGD). The online batch-means estimator of Zhu, Chen and Wu (2023) achieves an operator-norm convergence rate of

O(n^{-(1-α)/4})

, which yields

O(n^{-1/8})

at the optimal learning-rate exponent

α\rightarrow 1/2^+

. A rigorous per-block bias analysis reveals that re-tuning the block-growth parameter improves the batch-means rate to

O(n^{-(1-α)/3})

, achieving

O(n^{-1/6})

. The modified estimator requires no Hessian access and preserves

O(d^2)

memory. We provide a complete error decomposition into variance, stationarity bias, and nonlinearity bias components. A weighted-averaging variant that avoids hard truncation is also discussed. We establish the minimax rate

Θ(n^{-(1-α)/2})

for Hessian-free covariance estimation from the SGD trajectory: a Le Cam lower bound gives

Ω(n^{-(1-α)/2})

, and a trajectory-regression estimator--which estimates the Hessian by regressing SGD increments on iterates--achieves

O(n^{-(1-α)/2})

, matching the lower bound. The construction reveals that the bottleneck is the sublinear accumulation of information about the Hessian from the SGD drift.

Yijin Ni, Xiaoming Huo

H. Milton Stewart School of Industrial and Systems Engineering, Georgia Institute of Technology

Polyak-Ruppert Averaging Covariance Estimation Minimax Estimation Stochastic Gradient Descent

Ask PDF

Jun 14, 2026

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

Reliable state estimation in robotics and control re quires balancing estimation accuracy against computational cost. While filtering-based methods such as the Extended Kalman Filter (EKF) provide efficient real-time updates, and optimisation based formulations using factor graphs improve global consistency, the role of optimisation scheduling is often treated implicitly rather than examined as an explicit design variable. This paper presents an experimental study that explicitly isolates optimisation scheduling using a Smart Scheduled Hybrid (SSH) EKF-FGO framework as a controlled testbed. By combining EKF-based state propagation with periodically invoked batch optimisation and holding solver structure and effort fixed, the main contribution of this work is the experimental characterisation of optimisation scheduling as an independent design variable governing the trade-off between intermediate estimation accuracy and computational cost. Simulation results in a planar SLAM environment show that scheduling strongly influences pre optimisation drift, transient error behaviour, and runtime. In particular, the results identify operating regimes in which most of the benefit of global optimisation can be retained at a fraction of the computational cost, highlighting optimisation scheduling as an under-explored yet critical consideration in hybrid state estimation systems.

Eric Levy, Soosan Beheshti

Electrical, Computer, and Biomedical Engineering Toronto Metropolitan University Toronto, ON, Canada

Factor Graph Optimization Robot State Estimation

Ask PDF

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