Trouble with compass data

[trainman419]

I'm trying to incorporate compass data (absolute heading/orientation) into my kalman filter, and I'm not having any luck getting it to work properly.

From reading the source, it looks like graft doesn't take the orientation of an Odometry topic into account at all.

It also looks like the Imu input can return the orientation in the Imu message, but the absolute ukf doesn't use it.

Any thoughts on what the correct approach here is @chadrockey or @mikeferguson ?

[trainman419]

Scratch the comments about the Odometry data source; it looks like it's doing the correct thing, and the absolute UKF just isn't taking the orientation into account from either data source.

[trainman419]

I finally got this to converge reasonably well with the changes in #19

Blue is absolute pose (GPS + compass), Red is odometry, Green is the Graft output.