├── README.md
├── config
    └── imu_compass.yaml
├── launch
    └── imu_compass.launch
├── CMakeLists.txt
├── package.xml
├── CHANGELOG.rst
├── include
    └── imu_compass
    │   └── imu_compass.h
├── scripts
    └── compute_calibration
└── src
    └── imu_compass.cpp


/README.md:
--------------------------------------------------------------------------------
1 | imu_compass
2 | ===========
3 | 
4 | Node to combine IMU data (accelerometers and gyroscopes) with Compass data (magnetometers) for a cleaner reading of a Vehicles Heading
5 | 


--------------------------------------------------------------------------------
/config/imu_compass.yaml:
--------------------------------------------------------------------------------
1 | # Sample set of values. Please generate your own values using the compute_calibration script included in this package
2 | mag_bias:
3 |      x: 0.183117
4 |      y: 0.273127
5 |      z: -0.761884
6 |      radius: 0.271064
7 | 


--------------------------------------------------------------------------------
/launch/imu_compass.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 | <node pkg="imu_compass" type="imu_compass" name="imu_compass" output="screen" respawn="true">
3 |   <rosparam command="load" file="$(find imu_compass)/config/imu_compass.yaml"/>
4 | </node>
5 | </launch>
6 | 
7 | 
8 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(imu_compass)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs std_msgs tf)
 5 | 
 6 | catkin_package()
 7 | 
 8 | if(NOT WIN32)
 9 |   set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11")
10 | endif()
11 | 
12 | include_directories(include
13 |   ${catkin_INCLUDE_DIRS}
14 | )
15 | 
16 | add_executable(imu_compass src/imu_compass.cpp)
17 | target_link_libraries(imu_compass ${catkin_LIBRARIES})
18 | 
19 | install(
20 |   TARGETS imu_compass
21 |   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
22 | )
23 | 
24 | install(
25 |   PROGRAMS scripts/compute_calibration
26 |   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
27 | )
28 | 
29 | install(
30 |   DIRECTORY launch config
31 |   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
32 | )
33 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>imu_compass</name>
 4 |   <version>0.0.5</version>
 5 |   <description>Node to combine IMU data (accelerometers and gyroscopes) with Compass data (magnetometers) for a cleaner reading of a Vehicles Heading</description>
 6 |   <maintainer email="pmukherj@clearpathrobotics.com">Prasenjit Mukherjee</maintainer>
 7 | 
 8 |   <license>BSD</license>
 9 | 
10 |   <!-- Example: -->
11 |   <!-- <url type="website">http://ros.org/wiki/imu_compass</url> -->
12 | 
13 |   <buildtool_depend>catkin</buildtool_depend>
14 |   <build_depend>geometry_msgs</build_depend>
15 |   <build_depend>sensor_msgs</build_depend>
16 |   <build_depend>std_msgs</build_depend>
17 |   <build_depend>tf</build_depend>
18 |   <run_depend>geometry_msgs</run_depend>
19 |   <run_depend>sensor_msgs</run_depend>
20 |   <run_depend>std_msgs</run_depend>
21 |   <run_depend>tf</run_depend>
22 |   <run_depend>python-scipy</run_depend>
23 | 
24 |   <!-- The export tag contains other, unspecified, tags -->
25 |   <export>
26 | 
27 |   </export>
28 | </package>
29 | 


--------------------------------------------------------------------------------
/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package imu_compass
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 0.0.5 (2014-07-09)
 6 | ------------------
 7 | * removing bag references from sample hard-iron calibration output file
 8 | * fixing example launch file to reflect changes in param intake in the node
 9 | * Contributors: Prasenjit Mukherjee
10 | 
11 | 0.0.4 (2014-07-02)
12 | ------------------
13 | * successful test with kingfisher June 30 2014
14 | * adding the publishing of calibrated magnetometer data for debug purposes
15 | * Contributors: Mike Purvis, Prasenjit Mukherjee
16 | 
17 | 0.0.3 (2013-10-24)
18 | ------------------
19 | * Populate the curr_heading_float message with the declination-corrected heading value.
20 | * Add declination param and topic input to imu_compass node.
21 | * Add dependency on scipy for compute script.
22 | * Initialize curr_imu_reading_ with an empty message to avoid startup segfault.
23 | 
24 | 0.0.2 (2013-10-04)
25 | ------------------
26 | * adding rosparams for critical variables
27 | * tuning covariance to rely more on gyro
28 | 
29 | 0.0.1 (2013-09-23)
30 | ------------------
31 | * creating new package for imu_compass
32 | * added imu_compass.cpp, used to be called um6_compass when it came from https://github.com/clearpathrobotics/um6/tree/compass_cleanup/src
33 | 


--------------------------------------------------------------------------------
/include/imu_compass/imu_compass.h:
--------------------------------------------------------------------------------
 1 | /*imu_compass.cpp
 2 | Authors: Prasenjit (pmukherj@clearpathrobotics.com)
 3 | Copyright (c) 2013, Clearpath Robotics, Inc., All rights reserved.
 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 5 | following conditions are met:
 6 |  * Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 7 |    disclaimer.
 8 |  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
 9 |    disclaimer in the documentation and/or other materials provided with the distribution.
10 |  * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote products
11 |    derived from this software without specific prior written permission.
12 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
13 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
14 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
15 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
16 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
17 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
18 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
19 | 
20 | Description:
21 | Header file for IMU Compass Class that combines gyroscope and magnetometer data to get a clean estimate of yaw.
22 | */
23 | 
24 | #include "ros/ros.h"
25 | #include "tf/tf.h"
26 | #include "tf/transform_listener.h"
27 | 
28 | #include "sensor_msgs/Imu.h"
29 | #include "geometry_msgs/Vector3Stamped.h"
30 | #include "std_msgs/Float32.h"
31 | 
32 | //typedef boost::shared_ptr<sensor_msgs::Imu const> ImuConstPtr;
33 | 
34 | class IMUCompass {
35 | 
36 | private:
37 |   ros::NodeHandle node_;
38 |   ros::Subscriber imu_sub_;
39 |   ros::Subscriber mag_sub_;
40 |   ros::Subscriber decl_sub_;
41 |   ros::Publisher imu_pub_;
42 |   ros::Publisher mag_pub_;
43 |   ros::Publisher compass_pub_;
44 |   ros::Publisher raw_compass_pub_;
45 | 
46 |   tf::TransformListener listener_;
47 |   ros::Timer debug_timer_;
48 | 
49 |   void imuCallback(sensor_msgs::ImuPtr data);
50 |   void declCallback(const std_msgs::Float32& data);
51 |   void magCallback(const geometry_msgs::Vector3StampedConstPtr& data);
52 |   void debugCallback(const ros::TimerEvent&);
53 |   void repackageImuPublish(tf::StampedTransform);
54 | 
55 |   //Heading Filter functions
56 |   void initFilter(double heading_meas); //initialize heading fiter
57 |   bool first_mag_reading_; //signifies receiving the first magnetometer message
58 |   bool first_gyro_reading_; //signifies receiving the first gyroscope message
59 |   bool filter_initialized_; //after receiving the first measurement, make sure the filter is initialized
60 |   bool gyro_update_complete_; //sigfnifies that a gyro update (motion model update) has gone through
61 | 
62 |   double mag_zero_x_, mag_zero_y_, mag_zero_z_;
63 | 
64 |   sensor_msgs::ImuPtr curr_imu_reading_;
65 | 
66 |   //Heading Filter Variables
67 |   //State and Variance
68 |   double curr_heading_;
69 |   double curr_heading_variance_;
70 |   double sensor_timeout_;
71 | 
72 |   //Motion Update Variables
73 |   double heading_prediction_;
74 |   double heading_variance_prediction_;
75 |   double heading_prediction_variance_;
76 |   double mag_declination_;
77 |   double last_motion_update_time_;
78 |   double last_measurement_update_time_;
79 | 
80 |   //Measurement Update Variables
81 |   double yaw_meas_variance_;
82 | 
83 | public:
84 |   IMUCompass(ros::NodeHandle &n);
85 |   ~IMUCompass() {
86 |   }
87 | };
88 | 
89 | 


--------------------------------------------------------------------------------
/scripts/compute_calibration:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | 
  3 | import rosbag
  4 | 
  5 | import datetime
  6 | from tf.msg import tfMessage
  7 | from argparse import ArgumentParser
  8 | from geometry_msgs.msg import Quaternion
  9 | from numpy import mean, array, hypot, diff, convolve, arange, sin, cos, ones, pi, matrix
 10 | from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_multiply,quaternion_matrix
 11 | import tf
 12 | 
 13 | # Prompt user if scipy is missing.
 14 | try:
 15 |   from scipy import optimize
 16 | except ImportError:
 17 |   print("This script requires scipy be available.")
 18 |   print("On Ubuntu: sudo apt-get install python-scipy")
 19 |   exit(1)
 20 | 
 21 | # Plots are optional
 22 | try:
 23 |   from matplotlib import pyplot
 24 |   from mpl_toolkits.mplot3d import Axes3D
 25 | except ImportError:
 26 |   pyplot = None
 27 | 
 28 | parser = ArgumentParser(description='Process UM6 bag file for compass calibration. Pass a bag containing /imu/rpy and /imu/mag topics, with the UM6 compass facing upright, being slowly rotated in a clockwise direction for 30-120 seconds.')
 29 | parser.add_argument('bag', metavar='FILE', type=str, help='input bag file')
 30 | parser.add_argument('outfile', metavar='OUTFILE', type=str, help='output yaml file',
 31 |                     nargs="?", default="/tmp/calibration.yaml")
 32 | parser.add_argument('--plots', type=bool, help='Show plots if matplotlib available.')
 33 | args = parser.parse_args()
 34 | 
 35 | if not args.plots:
 36 |     pyplot = None
 37 | 
 38 | bag = rosbag.Bag(args.bag)
 39 | imu_rot = Quaternion(0,0,0,1) 
 40 | transform_found = False
 41 | for topic, msg, time in bag.read_messages(topics=("/tf", "/tf")):
 42 |         for c_trans in msg.transforms:
 43 | 		if ("base_link" in c_trans.header.frame_id and "imu_link" in c_trans.child_frame_id):
 44 | 			imu_rot = c_trans.transform.rotation
 45 | 			transform_found = True
 46 | 			break
 47 | 	if (transform_found):
 48 | 		break
 49 | 
 50 | temp_q = (imu_rot.x, imu_rot.y, imu_rot.z, imu_rot.w)
 51 | imu_rot = temp_q
 52 | t_array = quaternion_matrix(imu_rot)
 53 | t_mat = matrix(t_array)
 54 | t_mat = t_mat.I
 55 | 
 56 | if (not transform_found):
 57 | 	print("Transform between base_link and imu_link not found, assuming unity")
 58 | 
 59 | time_yaw_tuples = []
 60 | for topic, msg, time in bag.read_messages(topics=("/imu/rpy", "imu/rpy")):	
 61 | 	o_msg = matrix([msg.vector.x, msg.vector.y, msg.vector.z, 0])
 62 | 	o_msg = o_msg.T
 63 | 	transformed_msg = t_mat*o_msg
 64 | 	time_yaw_tuples.append((time.to_sec(), float(transformed_msg[2])))
 65 | 	
 66 | 
 67 | if len(time_yaw_tuples) < 100:
 68 |   print("Insufficient data or no data in bag file. Looking for topic /imu/rpy.")
 69 |   exit(1)
 70 | 
 71 | 
 72 | time_yaw = zip(*time_yaw_tuples)
 73 | time_yaw.append(diff(time_yaw[-1]))
 74 | time_yaw_tuples_filtered = [tup for tup in zip(*time_yaw) if abs(tup[-1]) < 3.0]
 75 | time_yaw = zip(*time_yaw_tuples_filtered)
 76 | # apply smoothing as a new column
 77 | w = [1.0 / 30.0] * 30
 78 | time_yaw.append(convolve(time_yaw[-1], w, 'same'))
 79 | 
 80 | # remove sections of no movement.
 81 | time_yaw_tuples_movement = [tup for tup in zip(*time_yaw)] # if abs(tup[-1]) > 0.01]
 82 | time_start = time_yaw_tuples_movement[50][0]
 83 | time_end = time_yaw_tuples_movement[-50][0]
 84 | 
 85 | if pyplot:
 86 |   fig = pyplot.figure()
 87 |   ax1 = fig.add_subplot(211)
 88 |   ax1.scatter(array(time_yaw[0]), time_yaw[-1])
 89 |   lines = pyplot.axvline(time_start)
 90 |   pyplot.axvline(time_end)
 91 |   fig.gca().add_artist(lines)
 92 | 
 93 | vecs = []
 94 | for topic, msg, time in bag.read_messages(topics=("/imu/mag", "imu/mag")):
 95 |   if time.to_sec() > time_start and time.to_sec() < time_end:
 96 | 	o_msg = matrix([msg.vector.x, msg.vector.y, msg.vector.z, 0])
 97 | 	o_msg = o_msg.T
 98 | 	t_msg = t_mat*o_msg
 99 | 	vecs.append((float(t_msg[0]), float(t_msg[1]), float(t_msg[2])))
100 | 
101 | print ("Using data from " + str(time_start) +" to " + str (time_end) + "(" + str(time_end - time_start) + " seconds), or " + str(len(vecs)) + " samples.")
102 | 
103 | def calc_R(xc, yc):
104 |     """ calculate the distance of each 2D points from the center (xc, yc) """
105 |     return hypot(x-xc, y-yc)
106 | 
107 | def f_2(c):
108 |     """ calculate the algebraic distance between the 2D points and the mean circle centered at c=(xc, yc) """
109 |     Ri = calc_R(*c)
110 |     return Ri - Ri.mean()
111 | 
112 | 
113 | x,y,z = zip(*vecs)
114 | center_estimate = mean(x), mean(y)
115 | center, ier = optimize.leastsq(f_2, center_estimate)
116 | radius = calc_R(*center).mean()
117 | center = (center[0], center[1], mean(z))
118 | 
119 | a = arange(0, 2*pi + pi/50, pi/50)
120 | circle_points = (center[0] + cos(a) * radius, 
121 |                  center[1] + sin(a) * radius, 
122 |                  center[2] * ones(len(a)))
123 | 
124 | print("Magnetic circle centered at " + str(center) + ", with radius " + str(radius))
125 | 
126 | with open(args.outfile, "w") as f:
127 |   f.write("# Generated from %s on %s.\n" % (args.bag, datetime.date.today()))
128 |   f.write("mag_bias:\n")
129 |   f.write("     x: %f\n" % center[0])
130 |   f.write("     y: %f\n" % center[1])
131 |   f.write("     z: %f\n" % center[2])
132 |   f.write("     radius: %f\n" % radius)
133 | 
134 | print("Calibration file written to " + args.outfile)
135 | 
136 | if pyplot:
137 |   ax2 = fig.add_subplot(212, projection='3d')
138 |   ax2.view_init(elev=80, azim=5)
139 |   ax2.scatter(x, y, z)
140 |   ax2.plot(*circle_points, c="red")
141 |   pyplot.show()
142 | 


--------------------------------------------------------------------------------
/src/imu_compass.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | imu_compass.cpp
  3 | Authors: Prasenjit (pmukherj@clearpathrobotics.com)
  4 | Copyright (c) 2013, Clearpath Robotics, Inc., All rights reserved.
  5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
  6 | following conditions are met:
  7 |  * Redistributions of source code must retain the above copyright notice, this list of conditions and the following
  8 |    disclaimer.
  9 |  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
 10 |    disclaimer in the documentation and/or other materials provided with the distribution.
 11 |  * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote products
 12 |    derived from this software without specific prior written permission.
 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 14 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 15 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 16 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 17 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 18 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 19 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 20 | 
 21 | Description:
 22 | CPP file for IMU Compass Class that combines gyroscope and magnetometer data to get a clean estimate of yaw.
 23 | */
 24 | 
 25 | #include "imu_compass/imu_compass.h"
 26 | 
 27 | double magn(tf::Vector3 a) {
 28 |       return sqrt(a.x()*a.x() + a.y()*a.y() + a.z()*a.z());
 29 | }
 30 | 
 31 | IMUCompass::IMUCompass(ros::NodeHandle &n) :
 32 |     node_(n), curr_imu_reading_(new sensor_msgs::Imu()) {
 33 |   // Acquire Parameters
 34 |   ros::param::param("~mag_bias/x", mag_zero_x_, 0.0);
 35 |   ros::param::param("~mag_bias/y", mag_zero_y_, 0.0);
 36 |   ros::param::param("~mag_bias/z", mag_zero_z_, 0.0);
 37 | 
 38 | 
 39 |   ROS_INFO("Using magnetometer bias (x,y):%f,%f", mag_zero_x_, mag_zero_y_);
 40 | 
 41 |   ros::param::param("~compass/sensor_timeout", sensor_timeout_, 0.5);
 42 |   ros::param::param("~compass/yaw_meas_variance", yaw_meas_variance_, 10.0);
 43 |   ros::param::param("~compass/gyro_meas_variance", heading_prediction_variance_, 0.01);
 44 |   ROS_INFO("Using variance %f", yaw_meas_variance_);
 45 | 
 46 |   ros::param::param("~compass/mag_declination", mag_declination_, 0.0);
 47 |   ROS_INFO("Using magnetic declination %f (%f degrees)", mag_declination_, mag_declination_ * 180 / M_PI);
 48 | 
 49 |   // Setup Subscribers
 50 |   imu_sub_ = node_.subscribe("imu/data", 1000, &IMUCompass::imuCallback, this);
 51 |   mag_sub_ = node_.subscribe("imu/mag", 1000, &IMUCompass::magCallback, this);
 52 |   decl_sub_ = node_.subscribe("imu/declination", 1000, &IMUCompass::declCallback, this);
 53 |   imu_pub_ = node_.advertise<sensor_msgs::Imu>("imu/data_compass", 1);
 54 |   compass_pub_ = node_.advertise<std_msgs::Float32>("imu/compass_heading", 1);
 55 |   mag_pub_ = node_.advertise<geometry_msgs::Vector3Stamped>("imu/mag_calib", 1);
 56 | 
 57 |   raw_compass_pub_ = node_.advertise<std_msgs::Float32>("imu/raw_compass_heading", 1);
 58 | 
 59 |   first_mag_reading_ = false;
 60 |   first_gyro_reading_ = false;
 61 |   gyro_update_complete_ = false;
 62 |   last_motion_update_time_ = ros::Time::now().toSec();
 63 |   debug_timer_ = node_.createTimer(ros::Duration(1), &IMUCompass::debugCallback, this);
 64 | 
 65 |   ROS_INFO("Compass Estimator Started");
 66 | }
 67 | 
 68 | void IMUCompass::debugCallback(const ros::TimerEvent&) {
 69 |   if (!first_gyro_reading_)
 70 |     ROS_WARN("Waiting for IMU data, no gyroscope data available)");
 71 |   if (!first_mag_reading_)
 72 |     ROS_WARN("Waiting for mag data, no magnetometer data available, Filter not initialized");
 73 | 
 74 |   if ((ros::Time::now().toSec() - last_motion_update_time_ > sensor_timeout_) && first_gyro_reading_) {
 75 |     // gyro data is coming in too slowly
 76 |     ROS_WARN("Gyroscope data being receieved too slow or not at all");
 77 |     first_gyro_reading_ = false;
 78 |   }
 79 | 
 80 |   if ((ros::Time::now().toSec() - last_measurement_update_time_ > sensor_timeout_) && first_mag_reading_) {
 81 |     // gyro data is coming in too slowly
 82 |     ROS_WARN("Magnetometer data being receieved too slow or not at all");
 83 |     filter_initialized_ = false;
 84 |     first_mag_reading_ = false;
 85 |   }
 86 | }
 87 | 
 88 | void IMUCompass::imuCallback(const sensor_msgs::ImuPtr data) {
 89 |   // Transform Data and get the yaw direction
 90 |   geometry_msgs::Vector3 gyro_vector;
 91 |   geometry_msgs::Vector3 gyro_vector_transformed;
 92 |   gyro_vector = data->angular_velocity;
 93 | 
 94 |   if(!first_gyro_reading_)
 95 |     first_gyro_reading_ = true;
 96 | 
 97 |   double dt = ros::Time::now().toSec() - last_motion_update_time_;
 98 |   last_motion_update_time_ = ros::Time::now().toSec();
 99 |   tf::StampedTransform transform;
100 | 
101 |   try {
102 |     listener_.lookupTransform("base_link", data->header.frame_id, ros::Time(0), transform);
103 |   } catch (tf::TransformException &ex) {
104 |     ROS_WARN("Missed transform between base_link and %s", data->header.frame_id.c_str());
105 |     return;
106 |   }
107 | 
108 |   tf::Vector3 orig_bt;
109 |   tf::Matrix3x3 transform_mat(transform.getRotation());
110 |   tf::vector3MsgToTF(gyro_vector, orig_bt);
111 |   tf::vector3TFToMsg(orig_bt * transform_mat, gyro_vector_transformed);
112 |   double yaw_gyro_reading =  gyro_vector_transformed.z;
113 | 
114 |   // Run Motion Update
115 |   if (filter_initialized_) {
116 |     heading_prediction_ = curr_heading_ + yaw_gyro_reading * dt;  // xp = A*x + B*u
117 |     heading_variance_prediction_ = curr_heading_variance_ + heading_prediction_variance_; // Sp = A*S*A' + R
118 | 
119 |     if (heading_prediction_ > 3.14159)
120 |       heading_prediction_ -= 2 * 3.14159;
121 |     else if(heading_prediction_ < -3.14159)
122 |       heading_prediction_ += 2 * 3.14159;
123 |     gyro_update_complete_ = true;
124 |   }
125 |   curr_imu_reading_ = data;
126 | }
127 | 
128 | void IMUCompass::declCallback(const std_msgs::Float32& data) {
129 |   mag_declination_ = data.data;
130 |   ROS_INFO("Using magnetic declination %f (%f degrees)", mag_declination_, mag_declination_ * 180 / M_PI);
131 | }
132 | 
133 | void IMUCompass::magCallback(const geometry_msgs::Vector3StampedConstPtr& data) {
134 |   geometry_msgs::Vector3 imu_mag = data->vector;
135 |   geometry_msgs::Vector3 imu_mag_transformed;
136 | 
137 |   // Check for nans and bail
138 |   if ( std::isnan(data->vector.x) ||
139 |        std::isnan(data->vector.y) ||
140 |        std::isnan(data->vector.z) ) {
141 |     return;
142 |   }
143 | 
144 |   imu_mag.x = data->vector.x;
145 |   imu_mag.y = data->vector.y;
146 |   imu_mag.z = data->vector.z;
147 | 
148 |   last_measurement_update_time_ = ros::Time::now().toSec();
149 |   tf::StampedTransform transform;
150 |   try {
151 |     listener_.lookupTransform("base_link", data->header.frame_id, ros::Time(0), transform);
152 |   } catch (tf::TransformException &ex) {
153 |     ROS_WARN("Missed transform between base_link and %s", data->header.frame_id.c_str());
154 |     return;
155 |   }
156 | 
157 |   tf::Vector3 orig_bt;
158 |   tf::Matrix3x3 transform_mat(transform.getRotation());
159 |   tf::vector3MsgToTF(imu_mag, orig_bt);
160 |   tf::vector3TFToMsg(orig_bt * transform_mat, imu_mag_transformed);
161 | 
162 |   // Compensate for hard iron
163 |   double mag_x = imu_mag_transformed.x - mag_zero_x_;
164 |   double mag_y = imu_mag_transformed.y - mag_zero_y_;
165 |   double mag_z = imu_mag_transformed.z;  // calibration is purely 2D
166 | 
167 |   // Normalize vector
168 |   tf::Vector3 calib_mag(mag_x, mag_y, mag_z);
169 |   calib_mag = calib_mag / magn(calib_mag);
170 |   mag_x = calib_mag.x();
171 |   mag_y = calib_mag.y();
172 |   mag_z = calib_mag.z();
173 | 
174 |   geometry_msgs::Vector3Stamped calibrated_mag;
175 |   calibrated_mag.header.stamp = ros::Time::now();
176 |   calibrated_mag.header.frame_id = "imu_link";
177 | 
178 |   calibrated_mag.vector.x = calib_mag.x();
179 |   calibrated_mag.vector.y = calib_mag.y();
180 |   calibrated_mag.vector.z = calib_mag.z();
181 | 
182 |   mag_pub_.publish(calibrated_mag);
183 | 
184 |   tf::Quaternion q;
185 |   tf::quaternionMsgToTF(curr_imu_reading_->orientation, q);
186 |   tf::Transform curr_imu_meas;
187 |   curr_imu_meas = tf::Transform(q, tf::Vector3(0, 0, 0));
188 |   curr_imu_meas = curr_imu_meas * transform;
189 |   tf::Quaternion orig (transform.getRotation());
190 | 
191 |   // Till Compensation
192 |   tf::Matrix3x3 temp(curr_imu_meas.getRotation());
193 | 
194 |   double c_r, c_p, c_y;
195 |   temp.getRPY(c_r, c_p, c_y);
196 |   double cos_pitch = cos(c_p);
197 |   double sin_pitch = sin(c_p);
198 |   double cos_roll = cos(c_r);
199 |   double sin_roll = sin(c_r);
200 |   double t_mag_x = mag_x * cos_pitch + mag_z * sin_pitch;
201 |   double t_mag_y = mag_x * sin_roll * sin_pitch + mag_y * cos_roll - mag_z * sin_roll * cos_pitch;
202 |   double head_x = t_mag_x;
203 |   double head_y = t_mag_y;
204 | 
205 |   // Retrieve magnetometer heading
206 |   double heading_meas = atan2(head_x, head_y);
207 | 
208 |   // If this is the first magnetometer reading, initialize filter
209 |   if (!first_mag_reading_) {
210 |     // Initialize filter
211 |     initFilter(heading_meas);
212 |     first_mag_reading_ = true;
213 |     return;
214 |   }
215 |   // If gyro update (motion update) is complete, run measurement update and publish imu data
216 |   if (gyro_update_complete_) {
217 |     // K = Sp*C'*inv(C*Sp*C' + Q)
218 |     double kalman_gain = heading_variance_prediction_ * (1 / (heading_variance_prediction_ + yaw_meas_variance_));
219 |     double innovation = heading_meas - heading_prediction_;
220 |     if (abs(innovation) > M_PI)  // large change, signifies a wraparound. kalman filters don't like discontinuities like wraparounds, handle seperately.
221 |       curr_heading_ = heading_meas;
222 |     else
223 |       curr_heading_ = heading_prediction_ + kalman_gain * (innovation); // mu = mup + K*(y-C*mup)
224 | 
225 |     curr_heading_variance_ = (1 - kalman_gain) * heading_variance_prediction_; // S = (1-K*C)*Sp
226 | 
227 |     std_msgs::Float32 raw_heading_float;
228 |     raw_heading_float.data = heading_meas;
229 |     raw_compass_pub_.publish(raw_heading_float);
230 | 
231 |     repackageImuPublish(transform);
232 |     gyro_update_complete_ = false;
233 |   }
234 | }
235 | 
236 | void IMUCompass::repackageImuPublish(tf::StampedTransform transform) {
237 |   // Get Current IMU reading and Compass heading
238 |   tf::Quaternion imu_reading;
239 |   tf::quaternionMsgToTF(curr_imu_reading_->orientation, imu_reading);
240 |   double compass_heading = curr_heading_ - mag_declination_;
241 | 
242 |   // Transform curr_imu_reading to base_link
243 |   tf::Transform o_imu_reading;
244 |   o_imu_reading = tf::Transform(imu_reading, tf::Vector3(0, 0, 0));
245 |   o_imu_reading = o_imu_reading * transform;
246 |   imu_reading = o_imu_reading.getRotation();
247 | 
248 |   // Acquire Quaternion that is the difference between the two readings
249 |   tf::Quaternion compass_yaw = tf::createQuaternionFromRPY(0.0, 0.0, compass_heading);
250 |   tf::Quaternion diff_yaw = tf::createQuaternionFromRPY(0.0, 0.0, compass_heading - tf::getYaw(imu_reading));
251 | 
252 |   // Transform the imu reading by the difference
253 |   tf::Quaternion new_quaternion = diff_yaw * imu_reading;
254 | 
255 |   // Transform the imu reading back into imu_link
256 |   sensor_msgs::Imu publish_imu;
257 |   o_imu_reading = tf::Transform(new_quaternion, tf::Vector3(0, 0, 0));
258 |   o_imu_reading = o_imu_reading * (transform.inverse());
259 |   tf::quaternionTFToMsg(o_imu_reading.getRotation(), curr_imu_reading_->orientation);
260 | 
261 |   // Publish all data
262 |   std_msgs::Float32 curr_heading_float;
263 |   curr_heading_float.data = compass_heading;
264 |   compass_pub_.publish(curr_heading_float);
265 |   imu_pub_.publish(curr_imu_reading_);
266 | }
267 | 
268 | void IMUCompass::initFilter(double heading_meas) {
269 |   curr_heading_ = heading_meas;
270 |   curr_heading_variance_ = 1; // not very sure
271 |   filter_initialized_ = true;
272 |   ROS_INFO("Magnetometer data received. Compass estimator initialized");
273 | }
274 | 
275 | int main(int argc, char **argv) {
276 |   ros::init(argc, argv, "imu_compass");
277 |   ros::NodeHandle node;
278 |   IMUCompass imu_heading_estimator(node);
279 |   ros::spin();
280 |   return 0;
281 | }
282 | 


--------------------------------------------------------------------------------