Added copy of ros::Tf::LinearMath (which was a copy from somewhere else in the
first place) to get RPY value out of quaternions.

Added an optimizer for the magnetometer parameters.

Added a pre-optimization step for magnetometer scaling and offset, and limited
optimization to magnetic declination. Working, but seg-fault on exit.