Sounds like you most know of what you need to do. I take it you have an /imu_raw topics (composed of raw sensor byte outputs), you should also publish an /imu that's scaled into SI units.
The right messages are:
http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html
http://docs.ros.org/api/sensor_msgs/html/msg/MagneticField.html
http://docs.ros.org/api/sensor_msgs/html/msg/FluidPressure.html
Some available attitude filters:
http://mirror.umd.edu/roswiki/imu_filter_madgwick.html
http://www.youtube.com/watch?v=BXsGWoOMtmU
(Maybe one day once I have enough free time/consistent schedule to finish my overhaul of ROS sensor fusion package, graft:)
https://github.com/chadrockey/graft/blob/hydro-devel/test_attitude.launch
One implementation that I know works well is Androids, it wouldn't be too difficult to wrap the AOSP source in a thin ROS wrapper:
https://android.googlesource.com/platform/frameworks/base/+/fca8947f496967ac2097366672a4c725f1b9311f/services/sensorservice/Fusion.cpp
https://android.googlesource.com/platform/frameworks/base/+/fca8947f496967ac2097366672a4c725f1b9311f/services/sensorservice/Fusion.h
Video of Android's algorithm:
http://www.youtube.com/watch?v=C7JQ7Rpwn2k
As for elevation, for my ROSCon talk, I just implemented a quick accumulator that smoothed the altitude offset from where I started the node. The algorithm for altitude from barometric pressure is here:
http://en.wikipedia.org/wiki/Atmospheric_pressure#Altitude_atmospheric_pressure_variation
One of these days, I'd like to see a ROS node that polls local barometric pressure references from airports using GPS.
Oh, and there are also a few miscellaneous tools here:
https://github.com/ros-perception/imu_pipeline
