# Calculating the covariance matrix of a measurement

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$

• $\sigma_x = \sqrt{\sigma^2_x}$ ?! – CroCo Sep 21 '17 at 6:44
• you mean $\sigma_{x}=\sqrt{(x_{gt}-x_{m})^2}$ ? – Daphnee Sep 21 '17 at 8:19

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.

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.

• Thanks but how to calculate $\sigma_{xx}$ in this case? I don't have the covariance matrix. I have to estimate $\sigma_{xx}$ , $\sigma_{yy}$ and $\sigma_{\theta\theta}$ first to estimate the covariance matrix. Sould it be done empirically and see how much my measurements are deviated from the ground truth instead of how much they are deviated from the mean? – Daphnee Sep 21 '17 at 11:54
• @user7281779, you misunderstand it. The co-variance matrix must be given in your calculation. It holds the uncertainty in the system. – CroCo Sep 21 '17 at 11:56
• But in my case it is not given..is there a possibility to evaluate it? – Daphnee Sep 21 '17 at 11:57
• you are eventually starting from initial values and then the robot builds its knowledge gradually based on the state vector and the uncertainty matrix. You need to elaborate a bit more about your problem then. – CroCo Sep 21 '17 at 12:06
• The measurement for the kalman filtering process is taken from a place recognition algorithm. The robot comapres its actual visual input to a database of images and returns the best match. To each image in the database is associated a pose from which the measurement $z=(x,y,\theta)$ can be extracted. I need to associate a covariance matrix to this measurement. Could the variance be extracted from the diffrence between the measured pose and the ground truth one? – Daphnee Sep 21 '17 at 12:19