Calculating the covariance matrix of a measurement

Question

I want to estimate the covariance matrix of a measurement for a robot evolving on plane and having the following state vector. $$ X = \begin{bmatrix} x\\ y\\ \theta \end{bmatrix} $$ $x$ and $y$ are the coordinates in $XY$ plane et $\theta$ the heading angle.

The measurement is taken from a place recognition algorithm which returns an pose $$z= \begin{bmatrix} x_{m}\\ y_{m}\\ \theta_{m} \end{bmatrix} $$

The covariance matrix of the measurement should have the following form

$$ \Sigma = \begin{bmatrix} \sigma_{x}^2 & 0 & 0 \\ 0 & \sigma_{y}^2 & 0 \\ 0 & 0 & \sigma_{\theta }^2 \\ \end{bmatrix} $$

How to calculate $\sigma_{x}$ for example given that I have the ground truth pose $$ground_{truth}= \begin{bmatrix} x_{gt}\\ y_{gt}\\ \theta_{gt} \end{bmatrix} $$ and the measurement $z$

Answer 1

Correct me if I'm wrong, but it sounds like you're looking for an estimate of the noise in the measurement, which is usually called 'R' in the Kalman filter (https://en.wikipedia.org/wiki/Kalman_filter#Underlying_dynamical_system_model). If you have the ground truth and you assume that the measurements have Gaussian noise (which is assumed in the Kalman filter) and are distributed about the ground truth measurements, you can get the MLE by doing

$ \hat{\sigma_{x}}^{2} = \frac{1}{n}\sum_{i=0}^{i=n} (x_{gt}^{i} - x_{m}^{i})^{2} $

To get $\sigma{x}$ you would just take the square root. You can do the same for the other variables of course. If you are keeping $\theta$ in a bounded range make sure you are careful about subtraction when doing this.

This would assume that all of the measurements come from a $\mathcal{N}(x_{gt}^{i}, \sigma_{x}^{2})$ distribution, where $x_{gt}^{i}$ is the ground truth measurement for the $i$-th measurement and $x_{m}^{i}$ is the noisy measured value. This probably won't be strictly true in practice because the variance will likely depend on the number/type of visual features in your environment and details in your place recognition module, but you'll at least be able to smooth out the measurements a bit. If you want to check how well this assumption holds, you can look at a histogram of the residuals $r_{i}$

$r_{i} = x_{gt}^{i} - x_{m}^{i}$

If the assumption is correct it should look roughly normal.

If you want to get into the weeds a bit more and do this a bit more 'properly', you can get estimates of the uncertainty in the transformation from your place recognition module by examining the fit that the module does. I won't go into it here since you didn't ask, but see Chapter 6.1.4 in Szeliski (Computer Vision: Algorithms and Applications) on 'uncertainty modeling' if you want to know more.

Answer 2

The co-variance matrix of the whole system reflects the uncertainty in the system. $\sigma_{xx}$ more precisely than $\sigma_x$ reflects the uncertainty in the x position. Since the off-diagonal elements are zeros (i.e. it is not always the case but for the sake of simplicity one assumes so), there is no correlation. This means the uncertainty in x position has no effect on the y position and the heading angle. The first element in the co-variance matrix is $\sigma^2_{xx}$ in your case, therefore, taking the square root of it, it gives you the uncertainty $\sigma_{xx}$ in the x position.