imu_filter_madgwick not changing orientation in odometry

edit

I'm trying to use imu_filter_madgwick to incorporate the /imu/mag channel data into the odometry to get an absolute orientation for input into an ekf for robot_navigation. I am using a Xsens imu so I had to write a small script to translate the output of /imu/mag to the message type imu_filter_madgwick expects.

#!/usr/bin/env python
import rospy

from geometry_msgs.msg import Vector3Stamped
from sensor_msgs.msg import MagneticField

def vector3Callback(msg):
    global mag_pub

    mag_msg = MagneticField()
    mag_msg.header = msg.header
    mag_msg.magnetic_field = msg.vector

    mag_pub.publish(mag_msg)

def main():
    global mag_pub

    rospy.init_node('mag_convert')
    rospy.Subscriber('/imu/magnetic_field', Vector3Stamped, vector3Callback)
    mag_pub = rospy.Publisher('/imu/mag', MagneticField, queue_size=10)
    rospy.spin()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass

I posted my issue on github here with the launch file and script along with a short bag file: https://github.com/ccny-ros-pkg/imu_t...

I'm asking here because I thought someone here might have encountered the same issue or has found a better way to incorporate the mag data into the localization.