Correct Handling of Differential Covariances

[ayrton04]

Currently, when a user sets a sensor to differential, the covariances, other than being rotated into the correct frame, are used as-is. This is not technically correct. Differential measurements are formed by taking the delta between the previous measurement and the current measurement, tacking them onto the current state estimate, and calling that a new measurement. The covariances should receive the same treatment, and should therefore be the current estimate error covariance combined with the covariance of the measurement delta.

Doing this is problematic, however, as it will involve multiple matrix inversion operations. A much better means of handling differential data (and one that I've experimented with previously) is to use the delta in measurements, combined with the time delta between the measurements, to generate twist messages and handle them as with any other twist data. Calculation of the covariances would be equally simple, and require less CPU time. Differential calculations should therefore be changed to operate this way.

While this method will be more correct in a numerical sense, it will also require that users take a bit more care in using the differential parameter, especially with orientation data. When raw positional velocities accumulate, their small errors also accumulate, resulting in absolute positional error. Over time, these errors can become significant. However, consider a robot that moves 20 meters and accumulates 1 meter of positional error. This seems reasonable, and won't result in any seemingly strange behavior. The same is not true for a robot that accumulates rotational error. Consider a robot that doesn't use an absolute orientation reference, but just accumulates rotational velocity. If that robot starts and immediately turns in place until it accumulates 1 radian of error, then drives straight for 20 meters, its final positional error will be much larger. In general, integration of rotational velocity is not as good as fusing absolute orientation measurements.

For users, what this means is that if you have only one source of absolute orientation data, you should fuse it with the differential parameter set to false. If you have multiple IMUs or sources of absolute rotational information, one of them should be fused with the differential parameter off, and the rest should have it on.

[ayrton04]

I am working on this and some other notable changes in the accuracy_fixes branch.

[ayrton04]

Completed merge of accuracy_fixes branch, which included this change.