Error in the covariance calculation when using the differential mode

[christiaantheunisse]

Describe the bug I suspect there is an error in the calculation of the covariance for the differential mode. The calculation is done over here. I think the sum of the covariances should be divided by the square of the delta timestep.

The math I was just trying to understand how the variances are calculated in the differential mode and I am definitely not an expert on this, but when I compare it to the following 1D example, I think it should be / dt^2 instead of * dt.

Suppose we have two measurements, which are normally distributed: $x_a \sim N(\mu_a, \sigma_a^2)$ at $t=0$ $x_b \sim N(\mu_b, \sigma_b^2)$ at $t=\Delta t$

The difference between the measurements would be: $\Delta x = (x_b - x_a) \sim N(\mu_b - \mu_a, \sigma_a^2 + \sigma_b^2)$

The velocity is then: $v = \frac{\Delta x}{\Delta t} \sim N(\frac{\mu_b - \mu_a}{\Delta t}, \frac{\sigma_a^2 + \sigma_b^2}{\Delta t ^ 2})$

So the variance of the velocity is: $\sigma_v = \frac{\sigma_a^2 + \sigma_b^2}{\Delta t ^ 2}$

I suspect that this will transfer to covariance matrices, although I am not sure about this.

However, I might just be entirely wrong, but I could not find any example or stack exchange question online solving with the same problem.

[ayrton04]

Doesn't your math assume two independent measurements?

Consider a robot with wheel encoders moving along a single axis. At time t = 1.0, the odometry measures X = 1.0 with variance sigma^2 = 0.01. At time t = 1.1, the odometry measures X = 1.01 with variance sigma^2 = 0.011. The wheel encoders are just accumulating error as they go, so the error has only grown a small amount in that time. So our resulting measurement, if we use your formulation, is X' = (1.01 - 1.0) / 0.1 = 0.1 m/s. The variance on that velocity measurement would be (0.01 + 0.11) / (0.1 * 0.1) = 12. That's a massive error for the velocity measurement that was derived over a very small window and with very high confidence measurements. The second variance is just the first variance with some more error integrated into it due to the wheel motion, so the two variables are highly dependent.

The variance when computing the difference of two dependent variables is $Var[X - Y] = Var[X] + Var[Y] - 2 Cov[X, Y]$. I suspect that the high level of correlation between the measurements at each time step would make $Var[X - Y]$ very small, such that the division by $\Delta t^2$ would produce an acceptable final variance for the velocity.

In any case, if you want to submit a PR to clean all that up, I'd welcome it, but we need to be careful. With the original formulation you posted, differentiating two measurements with very small variance increase will result in velocities that have such large errors that the filter will be extremely slow to converge.

[ayrton04]

Thinking on it some more, the formula variance_of_difference = variance_of_first_measurement + variance_of_second_measurement - 2 * variance_in_common. I think that variance_in_common here would be just variance_of_first_measurement, so the final formula would just be variance_of_second_measurement - variance_of_first_measurement (and then normalised by dt^2).

I need to sit down with a paper and work this all out properly, but I don't have the cycles right now.

[christiaantheunisse]

Thank you for your quick reply!

I indeed just realized that this will give a massive uncertainty. You are right that the assumption that the measurements are independent is wrong. Therefore, you need to make an assumption about the variance_in_common. However, I think this would depend on the sensors used, as illustrated by the following examples.

For example, when you consider an odometry estimate based on wheel encoders, the variance is slowing increasing. In that case, the assumption that the variance_in_common is the variance_of_first_measurement will at least produce some usable result.

However, when the position estimates are two SLAM measurements, the variance might be the same and the assumption that the variance_in_common is the variance_of_first_measurement will result in variance_of_difference = 0.

I think these examples illustrate that the problem is more complex and probably needs information that is simply not available to the EKF. I will let it rest for now and close the issue.

[ayrton04]

It's a good point. You'd need to add a parameter that says whether each sensor's measurements are independent or not. There's no way to encode that in any existing ROS message, AFAIK.