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

TL;DR

This work estimates the rate or schedule with which a sensor of known covariance must generate measurements to achieve specific estimation accuracy, and identifies scenarios where certain estimation accuracy is unachievable with the given system and sensor characteristics.

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 (5)

• Figure 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.
• Figure 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).
• Figure 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\,m$ and $\sigma_r = 0.08\,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.
• Figure 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.
• Figure 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.

Theorems & Definitions (3)

• Proposition 1
• proof
• Remark 1