Add support for compasses (absolute orientation)

[trainman419]

This is my attempted fix for #18.

I implemented the suggestion in http://answers.ros.org/question/9446/how-do-i-compute-the-covariance-matrix-for-an-orientation-sensor/ to generate the quaternion orientation covariance from the euler angle covariances.

It seems to work reasonably well on my data set, until something terrible happens and it diverges.

[trainman419]

I tried this with my other data sets, and it doesn't diverge.

[mikeferguson]

@trainman419 are these datasets (i presume they are bagfiles) somewhere accessible? I'd like to put together a series of actual rostests to use against my refactor, I have data for velocity filter, but not for the absolute case.

[chadrockey]

Looks about right.

The reason that this wasn't implemented is that I was still deciding how to handle orientation in the absolute case. Filtering quaternions is tricky business because the re-normalization step affects the covariances during the update and numerical stability is essential for all of these calculations.

If anyone has a suggestion or can find a reference, we should look at how this is implemented. I've seen just about everything from how this is implemented to running a directed cosines approach beside the UKF. The ultimate approach is probably finding a paper that documents a method to run the unscented transform on the quaternion without requiring normalization and maintaining numerical stability.

[trainman419]

I have data and some tools to use with it, but I don't have a properly tuned absolute filter yet. The divergence I was seeing was due to a bad GPS reading that was causing ridiculous position and velocity estimates; I've eliminated the offending data points, so I'm not seeing that problem any more.

I'm keeping my bag files here: http://namniart.com/dagny_data/ ; the log files I'm using for my current tests are loop_<N>_relative.bag, and the launch file I'm using is https://github.com/trainman419/dagny_ros/blob/master/dagny/graft_absolute.launch

Note that you'll also need my GPS to UTM node in https://github.com/trainman419/dagny_robomagellan

[trainman419]

I've been thinking about this more over the past few months, and I'm not terribly happy with the pseudo-inverse solution.

I think the better approach is to convert the input position to RPY, and then compute the covariance transformation matrix directly.

[trainman419]

I have a working version of a solution that doesn't use the pseudo-inverse, but it's based on my old branch and isn't ready to be merged yet because the formatting is messy and inconsistent.

Did you guys switch to tabs or something?

[mikeferguson]

We didn't switch to tabs -- I think it was mainly fixing the indentation as it was inconsistent and fixing various things to be style guide compatible, see #21