├── CMakeLists.txt
├── README.md
├── Screenshot from 2017-07-29 10-49-16.png
├── cfg
    ├── MyStuff.cfg
    └── MyStuff2.cfg
├── package.xml
└── src
    ├── Madgwick_filter.cpp
    ├── Mahony_filter.cpp
    └── bias_calculator.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(filter)
 3 | 
 4 | ## Compile as C++11, supported in ROS Kinetic and newer
 5 | # add_compile_options(-std=c++11)
 6 | 
 7 | ## Find catkin macros and libraries
 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 9 | ## is used, also find other catkin packages
10 | 
11 | find_package(catkin REQUIRED COMPONENTS
12 |   roscpp
13 |   rospy
14 |   sensor_msgs geometry_msgs tf dynamic_reconfigure
15 | )
16 | 
17 | generate_dynamic_reconfigure_options(
18 |   cfg/MyStuff.cfg
19 |   cfg/MyStuff2.cfg 
20 | )
21 | 
22 | 
23 | catkin_package(
24 | #  INCLUDE_DIRS include
25 | #  LIBRARIES temp
26 | #  CATKIN_DEPENDS roscpp
27 | #  DEPENDS system_lib
28 | )
29 | 
30 | include_directories(
31 | # include
32 |   ${catkin_INCLUDE_DIRS}
33 | )
34 | 
35 | 
36 | 
37 | 
38 | add_executable(Mahony_filter src/Mahony_filter.cpp)
39 | add_dependencies(Mahony_filter ${PROJECT_NAME}_gencfg)
40 | target_link_libraries(Mahony_filter ${catkin_LIBRARIES} )
41 | 
42 | 
43 | add_executable(Madgwick_filter src/Madgwick_filter.cpp)
44 | add_dependencies(Madgwick_filter ${PROJECT_NAME}_gencfg)
45 | target_link_libraries(Madgwick_filter ${catkin_LIBRARIES} )
46 | 
47 | add_executable(bias_calculator src/bias_calculator.cpp)
48 | add_dependencies(bias_calculator ${PROJECT_NAME}_gencfg)
49 | target_link_libraries(bias_calculator ${catkin_LIBRARIES} )
50 | 
51 | 
52 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # imu_filter
 2 | 
 3 | This is a project that uses IMU filter algorithm in ROS(Robot Operating System). The IMU I use is ICM 20602, consisting of a tri-axis gyroscope and a tri-axis accelerometer.
 4 | 
 5 | ## ROS API  
 6 | ### Subscribed Topics
 7 | Message containing raw IMU data, including angular velocities and linear accelerations.</br>
 8 | `imu0`(sensor_msgs/Imu) 
 9 | ### Published Topics
10 | The fused pose representation.</br>
11 | `quaternion`(geometry_msg/QuaternionStamped)</br>
12 | `ypr`(geometry_msgs/Vector3Stamped)  
13 | ## Parameters
14 | ### Dynamically Reconfigurable Parameters</br>
15 | (Mahony_filter)  `twoKp` `twoKi`</br>
16 | (Madgwick_filter) `beta`</br>
17 | To tune the dynamic parameters after runnig the node, input
18 | 
19 |     rosrun rqt_configure rqt_reconfigure
20 | According to Madgwick's thesis, the suggested beta = sqrt(3.0f / 4.0f) * gyroMeasError. ICM20602's gyroscope 
21 | sensitivity erroris ±1%. Thus the default beta is setting to 0.1088. As the specific application, the value
22 | can be tuned accoring to response and requriments.
23 | ### Non-dynamically Reconfigurable Parameters </br>
24 | `sampleFreq`(float, default: 400.0)</br>
25 | You can change its value at the beginning, Such as: 
26 |           
27 |     rosrun filter Madgwick_filter _sampleFreq:=200
28 | ## provied tf Transforms
29 | odom -> imu  
30 | So you can open rviz and set the fixed frame "odom", add TF then you can use it to verify the effect directly.
31 | ![Alt Text](https://github.com/marooncn/imu_filter/blob/master/Screenshot%20from%202017-07-29%2010-49-16.png)
32 | ## Result
33 | The Madgwick_filter node's effect is acceptable, but the Mahony_filter is disappointing.
34 | 
35 | 


--------------------------------------------------------------------------------
/Screenshot from 2017-07-29 10-49-16.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/marooncn/imu_filter/05a3c074285b4d25215c04ea1d23506e15dbc168/Screenshot from 2017-07-29 10-49-16.png


--------------------------------------------------------------------------------
/cfg/MyStuff.cfg:
--------------------------------------------------------------------------------
 1 | #! /usr/bin/env python
 2 | 
 3 | PACKAGE = 'filter'
 4 | 
 5 | 
 6 | from dynamic_reconfigure.parameter_generator_catkin import *
 7 | 
 8 | gen = ParameterGenerator()
 9 | 
10 | gen.add("twoKp", 'double', 0, "2* accelerometer proportional gain", 0.01, 0, 0.1);
11 | gen.add("twoKi", 'double', 0, "2* integral gain", 0.002, 0, 0.01);
12 | 
13 | exit(gen.generate(PACKAGE, "filter", "MyStuff"))
14 |  
15 | 


--------------------------------------------------------------------------------
/cfg/MyStuff2.cfg:
--------------------------------------------------------------------------------
 1 | #! /usr/bin/env python
 2 | 
 3 | PACKAGE = 'filter'
 4 | 
 5 | 
 6 | from dynamic_reconfigure.parameter_generator_catkin import *
 7 | 
 8 | gen = ParameterGenerator()
 9 | 
10 | gen.add("beta", 'double', 0, "2* accelerometer proportional gain", 0.1088, 0, 0.5);
11 | 
12 | 
13 | exit(gen.generate(PACKAGE, "filter", "MyStuff2"))
14 |  
15 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>filter</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The filter package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="maroon@todo.todo">maroon</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/filter</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *_depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use build_depend for packages you need at compile time: -->
35 |   <!--   <build_depend>message_generation</build_depend> -->
36 |   <!-- Use buildtool_depend for build tool packages: -->
37 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
38 |   <!-- Use run_depend for packages you need at runtime: -->
39 |   <!--   <run_depend>message_runtime</run_depend> -->
40 |   <!-- Use test_depend for packages you need only for testing: -->
41 |   <!--   <test_depend>gtest</test_depend> -->
42 |   <buildtool_depend>catkin</buildtool_depend>
43 |   <build_depend>roscpp</build_depend>
44 |   <build_depend>rospy</build_depend>
45 |   <build_depend>sensor_msgs</build_depend>
46 |   <build_depend>geometry_msgs</build_depend>
47 |   <build_depend>tf</build_depend>
48 |   <build_depend>dynamic_reconfigure</build_depend>
49 |   <run_depend>roscpp</run_depend>
50 |   <run_depend>rospy</run_depend>
51 |   <run_depend>sensor_msgs</run_depend>
52 |   <run_depend>geometry_msgs</run_depend>
53 |   <run_depend>tf</run_depend>
54 |   <run_depend>dynamic_reconfigure</run_depend>
55 | 
56 | 
57 |   <!-- The export tag contains other, unspecified, tags -->
58 |   <export>
59 |     <!-- Other tools can request additional information be placed here -->
60 | 
61 |   </export>
62 | </package>
63 | 


--------------------------------------------------------------------------------
/src/Madgwick_filter.cpp:
--------------------------------------------------------------------------------
  1 | #include "ros/ros.h"
  2 | #include "std_msgs/Float64.h"
  3 | #include "std_msgs/Header.h"
  4 | #include "geometry_msgs/Quaternion.h"
  5 | #include "geometry_msgs/QuaternionStamped.h"
  6 | #include "geometry_msgs/Vector3Stamped.h"
  7 | #include "geometry_msgs/TransformStamped.h"
  8 | #include "sensor_msgs/Imu.h"
  9 | #include "tf/transform_broadcaster.h"
 10 | #include "tf/transform_datatypes.h"
 11 | #include "tf/transform_listener.h"
 12 | #include "math.h"
 13 | #include "ros/console.h"
 14 | #include "dynamic_reconfigure/server.h"
 15 | #include "filter/MyStuff2Config.h"
 16 | 
 17 | 
 18 | using namespace std_msgs;
 19 | 
 20 | geometry_msgs::QuaternionStamped q;
 21 | geometry_msgs::Vector3Stamped v;
 22 | geometry_msgs::TransformStamped q_trans;
 23 | float sampleFreq;
 24 | double beta; 
 25 | float q0=1.0, q1=0.0, q2=0.0, q3=0.0;
 26 | Header header;
 27 | float ax, ay, az, gx, gy, gz;
 28 | ros::Duration dtime;
 29 | float dt;
 30 | 
 31 | float invSqrt(float );
 32 | void qua2Euler(geometry_msgs::QuaternionStamped );
 33 | void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
 34 | 
 35 | void callback(filter::MyStuff2Config &config, uint32_t level) {
 36 |   beta = config.beta;
 37 | 
 38 | }
 39 | 
 40 | 
 41 | void filter_function(const sensor_msgs::Imu& msg)
 42 | {
 43 | //  ros::Time start_time = ros::Time::now();
 44 | 
 45 | 
 46 |   header = msg.header;
 47 | //  q.linear_acceleration = msg.linear_acceleration;
 48 | //  q.angular_velocity = msg.angular_velocity;
 49 |   ax = msg.linear_acceleration.x;
 50 |   ay = msg.linear_acceleration.y;
 51 |   az = msg.linear_acceleration.z;
 52 |   gx = msg.angular_velocity.x;
 53 |   gy = msg.angular_velocity.y;
 54 |   gz = msg.angular_velocity.z;
 55 | 
 56 |  /* ros::Time this_time = header.stamp;
 57 |   static ros::Time last_time = header.stamp;
 58 |   dtime = this_time - last_time;
 59 |   dt = dtime.toNSec();
 60 |   ROS_INFO(" this_time = %f", this_time.toSec());
 61 |   ROS_INFO(" last_time = %f", last_time.toSec());
 62 |   dt = dt/1.0e9;
 63 |   // dt = 0.0025;  
 64 |   ROS_INFO(" dt = %f", dt);
 65 |   last_time = this_time; */
 66 |  
 67 |   // since we are using 1000 degrees/seconds range and the register is 16 bits
 68 |   // -1000 maps to a raw value of -32768
 69 |   // +1000 maps to a raw value of 32767
 70 |   gx = gx*1000.0/32768.0;
 71 |   gy = gy*1000.0/32768.0;
 72 |   gz = gz*1000.0/32768.0;
 73 | 
 74 |   //change Gyroscope units to radians/second, accelerometer units are irrelevant as the vector is normalised.
 75 |   // Convert gyroscope degrees/sec to radians/sec
 76 |   gx *= 0.0174533f;
 77 |   gy *= 0.0174533f;
 78 |   gz *= 0.0174533f;
 79 | 
 80 |   ROS_INFO("The original gx=%f, gy=%f, gz=%f ", gx, gy, gz);
 81 |   ROS_INFO("beta=%f", beta);
 82 | 
 83 |   MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az) ;
 84 | 
 85 |   q.header = header;
 86 |   q.quaternion.w = q0;
 87 |   q.quaternion.x = q1;
 88 |   q.quaternion.y = q2;
 89 |   q.quaternion.z = q3;
 90 | 
 91 |   ROS_INFO(" q0=%f, q1=%f, q2=%f, q3=%f ", q0, q1, q2, q3);
 92 |   qua2Euler(q);
 93 | //  ros::Time end_time = ros::Time::now();
 94 | //  double interval_time = (end_time - start_time).toNSec();
 95 |  // ROS_INFO("the  interval time is %lf ns" , interval_time);
 96 | }
 97 | 
 98 |   
 99 | int main(int argc, char **argv)
100 | {
101 |   ros::init(argc, argv, "Mahony_filter");
102 |   ros::NodeHandle n;
103 |   ros::Publisher pub1 = n.advertise<geometry_msgs::QuaternionStamped>("quaternion", 1);
104 |   ros::Publisher pub2 = n.advertise<geometry_msgs::Vector3Stamped>("ypr", 1);
105 |   tf::TransformBroadcaster q_broadcaster;
106 |   ros::Subscriber sub = n.subscribe("imu0", 10, filter_function);
107 |   if(!n.getParam("sampleFreq", sampleFreq))
108 |     sampleFreq = 400.0;
109 |   dynamic_reconfigure::Server<filter::MyStuff2Config> server;
110 |   dynamic_reconfigure::Server<filter::MyStuff2Config>::CallbackType f;
111 |   f = boost::bind(&callback, _1, _2);
112 |   server.setCallback(f);
113 | 
114 |   ros::Rate r(sampleFreq);
115 |   while(ros::ok())
116 |   {
117 |     pub1.publish(q);
118 |     pub2.publish(v);
119 |     ros::spinOnce();
120 |    
121 |     q_trans.header.stamp = header.stamp;
122 |     q_trans.header.frame_id = "/odom";
123 |     q_trans.child_frame_id = "/imu";
124 | /*  q_trans.transform.rotation.w = q.quaternion.w;
125 |   q_trans.transform.rotation.x = q.quaternion.x;
126 |   q_trans.transform.rotation.y = q.quaternion.y;
127 |   q_trans.transform.rotation.z = q.quaternion.z; */
128 |   q_trans.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(v.vector.z ,v.vector.y, v.vector.x);
129 | 
130 |   q_broadcaster.sendTransform(q_trans);
131 |   r.sleep();
132 |   }
133 |   return 0;
134 | }
135 | 
136 | //Fast inverse square root
137 | //reference: https://en.wikipedia.org/wiki/Fast_inverse_square_root
138 | float invSqrt(float x)
139 | {
140 |   float xhalf = 0.5f * x;
141 |   union
142 |   {
143 |     float x;
144 |     int i;
145 |   } u;
146 |   u.x = x;
147 |   u.i = 0x5f3759df - (u.i >> 1);
148 |   /* The next line can be repeated any number of times to increase accuracy */
149 |   u.x = u.x * (1.5f - xhalf * u.x * u.x);
150 |   return u.x;
151 | }
152 | 
153 | //reference: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
154 | void qua2Euler(geometry_msgs::QuaternionStamped q) {
155 | 
156 |   float x,y,z,w;
157 |   
158 |   x = q.quaternion.x;
159 |   y = q.quaternion.y;
160 |   z = q.quaternion.z;
161 |   w = q.quaternion.w;
162 | 
163 |   v.header = q.header;
164 | 
165 | //yaw (z-axis rotation)
166 |   float t0 = 2.0*(w*z+x*y);
167 |   float t1 = 1.0-2.0*(y*y+z*z);
168 |   v.vector.x = atan2(t0,t1); //the unit is radians
169 | //pitch (y-axis rotation)
170 |   float t2 = 2.0*(w*y-z*x);
171 |   t2 = (t2 > 1.0) ? 1.0 : t2;
172 |   t2 = (t2 < -1.0)? -1.0 : t2;
173 |   v.vector.y = asin(t2);  
174 | //roll (x-axis rotation)
175 |   float t3 = 2.0*(w*x+y*z);
176 |   float t4 = 1.0-2.0*(x*x+y*y);
177 |   v.vector.z = atan2(t3, t4);
178 | } 
179 | 
180 | 
181 | //Implementation of Madgwick's IMU and AHRS algorithms.
182 | //reference: An efficient orientation filter for inertial and inertial/magnetic sensor arrays. (This thesis's appendix has more detailed code and illutration)
183 | //reference: http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
184 | 
185 | void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {  
186 |   float recipNorm;    
187 |   float s0, s1, s2, s3;   // estimated direction of the gyroscope error
188 |   float qDot1, qDot2, qDot3, qDot4;    // quaternion derrivative from gyroscopes elements
189 |   float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;  //Axulirary variables to avoid reapeated calcualtions
190 |       
191 |   // Rate of change of quaternion from gyroscope  
192 |   qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);  
193 |   qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);  
194 |   qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);  
195 |   qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);  
196 |       
197 |   // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)  
198 |   if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {  
199 |       
200 |      // Normalise accelerometer measurement  
201 |      recipNorm = invSqrt(ax * ax + ay * ay + az * az);  
202 |      ax *= recipNorm;  
203 |      ay *= recipNorm;  
204 |      az *= recipNorm;     
205 |       
206 |      // Auxiliary variables to avoid repeated arithmetic  
207 |      _2q0 = 2.0f * q0;  
208 |      _2q1 = 2.0f * q1;  
209 |      _2q2 = 2.0f * q2;  
210 |      _2q3 = 2.0f * q3;  
211 |      _4q0 = 4.0f * q0;  
212 |      _4q1 = 4.0f * q1;  
213 |      _4q2 = 4.0f * q2;  
214 |      _8q1 = 8.0f * q1;  
215 |      _8q2 = 8.0f * q2;  
216 |      q0q0 = q0 * q0;  
217 |      q1q1 = q1 * q1;  
218 |      q2q2 = q2 * q2;  
219 |      q3q3 = q3 * q3;  
220 |       
221 |       // Gradient decent algorithm corrective step  
222 |       s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;  
223 |       s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;  
224 |       s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;  
225 |       s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; 
226 |  
227 |       recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // Normalise the gradient
228 |       s0 *= recipNorm;  
229 |       s1 *= recipNorm;  
230 |       s2 *= recipNorm;  
231 |       s3 *= recipNorm;  
232 | 
233 |       
234 |       // Apply feedback step  
235 |        qDot1 -= beta * s0;  
236 |        qDot2 -= beta * s1;  
237 |        qDot3 -= beta * s2;  
238 |        qDot4 -= beta * s3;  
239 |     }  
240 |       
241 |         // Integrate rate of change of quaternion to yield quaternion  
242 |      q0 += qDot1 * (1.0f / sampleFreq);  
243 |      q1 += qDot2 * (1.0f / sampleFreq);  
244 |      q2 += qDot3 * (1.0f / sampleFreq);  
245 |      q3 += qDot4 * (1.0f / sampleFreq);  
246 |       
247 |         // Normalise quaternion  
248 |      recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);  
249 |      q0 *= recipNorm;  
250 |      q1 *= recipNorm;  
251 |      q2 *= recipNorm;  
252 |      q3 *= recipNorm;  
253 | }  
254 | 
255 | 
256 | 
257 |   
258 |    
259 | 
260 | 


--------------------------------------------------------------------------------
/src/Mahony_filter.cpp:
--------------------------------------------------------------------------------
  1 | #include "ros/ros.h"
  2 | #include "std_msgs/Float64.h"
  3 | #include "std_msgs/Header.h"
  4 | //#include "std_msgs/Float64MultiArray.h"
  5 | #include "geometry_msgs/Quaternion.h"
  6 | #include "geometry_msgs/QuaternionStamped.h"
  7 | #include "geometry_msgs/Vector3Stamped.h"
  8 | #include "geometry_msgs/TransformStamped.h"
  9 | #include "sensor_msgs/Imu.h"
 10 | #include "tf/transform_broadcaster.h"
 11 | #include "tf/transform_datatypes.h"
 12 | #include "tf/transform_listener.h"
 13 | #include "math.h"
 14 | #include "ros/console.h"
 15 | #include "dynamic_reconfigure/server.h"
 16 | #include "filter/MyStuffConfig.h"
 17 | 
 18 | 
 19 | using namespace std_msgs;
 20 | 
 21 | geometry_msgs::QuaternionStamped q;
 22 | geometry_msgs::Vector3Stamped v;
 23 | geometry_msgs::TransformStamped q_trans;
 24 | double twoKp;  //2* accelerometer proportional gain
 25 | double twoKi;  //2* integral gain
 26 | float sampleFreq;
 27 | float q0=1.0, q1=0.0, q2=0.0, q3=0.0;
 28 | float ex_int=0.0, ey_int=0.0, ez_int=0.0;
 29 | Header header;
 30 | float ax, ay, az, gx, gy, gz;
 31 | ros::Duration dtime;
 32 | float dt;
 33 | 
 34 | 
 35 | float invSqrt(float );
 36 | void qua2Euler(geometry_msgs::QuaternionStamped );
 37 | void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
 38 | 
 39 | void callback(filter::MyStuffConfig &config, uint32_t level) {
 40 |   twoKp = config.twoKp;
 41 |   twoKi = config.twoKi;
 42 |   ROS_INFO("twoKi=%f, twoKp=%f", twoKi, twoKp);
 43 | }
 44 | 
 45 | 
 46 | //Implementation of Mahony&Madgwick's IMU and AHRS algorithms.
 47 | //reference: http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
 48 | void filter_function(const sensor_msgs::Imu& msg)
 49 | {
 50 |  // ros::Time start_time = ros::Time::now();
 51 | 
 52 |   header = msg.header;
 53 | //  q.linear_acceleration = msg.linear_acceleration;
 54 | //  q.angular_velocity = msg.angular_velocity;
 55 |   ax = msg.linear_acceleration.x;
 56 |   ay = msg.linear_acceleration.y;
 57 |   az = msg.linear_acceleration.z;
 58 |   gx = msg.angular_velocity.x;
 59 |   gy = msg.angular_velocity.y;
 60 |   gz = msg.angular_velocity.z;
 61 | 
 62 | /*  ros::Time this_time = header.stamp;
 63 |   static ros::Time last_time = header.stamp;
 64 |   dtime = this_time - last_time;
 65 |   dt = dtime.toNSec();
 66 |   ROS_INFO(" this_time = %f", this_time.toSec());
 67 |   ROS_INFO(" last_time = %f", last_time.toSec());
 68 |   dt = dt/1.0e9;
 69 |   // dt = 0.0025;  
 70 |   ROS_INFO(" dt = %f", dt);
 71 |   last_time = this_time;   */
 72 |  
 73 |   // since we are using 1000 degrees/seconds range and the register is 16 bits
 74 |   // -1000 maps to a raw value of -32768
 75 |   // +1000 maps to a raw value of 32767
 76 |   gx = gx*1000.0/32768.0;
 77 |   gy = gy*1000.0/32768.0;
 78 |   gz = gz*1000.0/32768.0;
 79 | 
 80 |   //change Gyroscope units to radians/second, accelerometer units are irrelevant as the vector is normalised.
 81 |   // Convert gyroscope degrees/sec to radians/sec
 82 |   gx *= 0.0174533f;
 83 |   gy *= 0.0174533f;
 84 |   gz *= 0.0174533f;
 85 | 
 86 |   ROS_INFO("The original gx=%f, gy=%f, gz=%f ", gx, gy, gz);
 87 |   
 88 |   MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
 89 |  
 90 |   q.header = header;
 91 |   q.quaternion.w = q0;
 92 |   q.quaternion.x = q1;
 93 |   q.quaternion.y = q2;
 94 |   q.quaternion.z = q3;
 95 | 
 96 |   ROS_INFO(" q0=%f, q1=%f, q2=%f, q3=%f ", q0, q1, q2, q3);
 97 |   qua2Euler(q); 
 98 |  //  ros::Time end_time = ros::Time::now();
 99 | //  double interval_time = (end_time - start_time).toNSec();
100 |  // ROS_INFO("the  interval time is %lf ns" , interval_time);
101 | }
102 | 
103 |   
104 | int main(int argc, char **argv)
105 | {
106 |   ros::init(argc, argv, "Mahony_filter");
107 |   ros::NodeHandle n;
108 |   ros::Publisher pub1 = n.advertise<geometry_msgs::QuaternionStamped>("quaternion", 1);
109 |   ros::Publisher pub2 = n.advertise<geometry_msgs::Vector3Stamped>("ypr", 1);
110 |  // ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
111 |   tf::TransformBroadcaster q_broadcaster;
112 |   //ros::Publisher pub3 = n.advertise<std_msgs::Float64MultiArray>("DCM",20);
113 |   //setFullSacleGyroRange(ICM20602_GYRO_RANGE_1000);
114 |   ros::Subscriber sub = n.subscribe("imu0", 10, filter_function);
115 |   if(!n.getParam("sampleFreq", sampleFreq))
116 |     sampleFreq = 400.0;
117 |   dt = 1.0/sampleFreq;
118 | 
119 |   dynamic_reconfigure::Server<filter::MyStuffConfig> server;
120 |   dynamic_reconfigure::Server<filter::MyStuffConfig>::CallbackType f;
121 |   f = boost::bind(&callback, _1, _2);
122 |   server.setCallback(f);
123 | 
124 |   ros::Rate r(sampleFreq);
125 |   while(ros::ok())
126 |   {
127 |     pub1.publish(q);
128 |     pub2.publish(v);
129 |     ros::spinOnce();
130 |    
131 |     q_trans.header.stamp = header.stamp;
132 |     q_trans.header.frame_id = "/odom";
133 |     q_trans.child_frame_id = "/imu";
134 | /*  q_trans.transform.rotation.w = q.quaternion.w;
135 |   q_trans.transform.rotation.x = q.quaternion.x;
136 |   q_trans.transform.rotation.y = q.quaternion.y;
137 |   q_trans.transform.rotation.z = q.quaternion.z; */
138 |   q_trans.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(v.vector.z ,v.vector.y, v.vector.x);
139 | 
140 |   q_broadcaster.sendTransform(q_trans);
141 |   r.sleep();
142 |   }
143 |   return 0;
144 | }
145 | 
146 | //Fast inverse square root
147 | //reference: https://en.wikipedia.org/wiki/Fast_inverse_square_root
148 | float invSqrt(float x)
149 | {
150 |   float xhalf = 0.5f * x;
151 |   union
152 |   {
153 |     float x;
154 |     int i;
155 |   } u;
156 |   u.x = x;
157 |   u.i = 0x5f3759df - (u.i >> 1);
158 |   /* The next line can be repeated any number of times to increase accuracy */
159 |   u.x = u.x * (1.5f - xhalf * u.x * u.x);
160 |   return u.x;
161 | }
162 | 
163 | //reference: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
164 | void qua2Euler(geometry_msgs::QuaternionStamped q) {
165 | 
166 |   float x,y,z,w;
167 |   
168 |   x = q.quaternion.x;
169 |   y = q.quaternion.y;
170 |   z = q.quaternion.z;
171 |   w = q.quaternion.w;
172 | 
173 |   v.header = q.header;
174 | 
175 | //yaw (z-axis rotation)
176 |   float t0 = 2.0*(w*z+x*y);
177 |   float t1 = 1.0-2.0*(y*y+z*z);
178 |   v.vector.x = atan2(t0,t1)*57.29578; //the unit is degree
179 | //pitch (y-axis rotation)
180 |   float t2 = 2.0*(w*y-z*x);
181 |   t2 = (t2 > 1.0) ? 1.0 : t2;
182 |   t2 = (t2 < -1.0)? -1.0 : t2;
183 |   v.vector.y = asin(t2)*57.29578;  
184 | //roll (x-axis rotation)
185 |   float t3 = 2.0*(w*x+y*z);
186 |   float t4 = 1.0-2.0*(x*x+y*y);
187 |   v.vector.z = atan2(t3, t4)*57.29578;
188 | 
189 | } 
190 | 
191 | void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
192 | 
193 |   float recipNorm; 
194 |   float halfvx, halfvy, halfvz;
195 |   float halfex, halfey, halfez;
196 |    float thetax, thetay, thetaz;
197 |   float dq0, dq1, dq2, dq3;
198 | 
199 |    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
200 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
201 |   //normalize the accelerometer vector
202 |   recipNorm = invSqrt(ax*ax+ay*ay+az*az);
203 |   ax *= recipNorm;
204 |   ay *= recipNorm;
205 |   az *= recipNorm;  
206 | 
207 |   ROS_INFO(" ax=%f, ay=%f, az=%f ", ax, ay, az);
208 | 
209 |   //calculate the direction of gravity according to quaternion
210 |   halfvx = q1*q3 - q0*q2;
211 |   halfvy = q0*q1 + q2*q3;
212 | // vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
213 |   halfvz = q0*q0 -0.5f + q3*q3;
214 | 
215 |   ROS_INFO("The halfvx=%f, halfvy=%f, halfvz=%f", halfvx, halfvy, halfvz);
216 | 
217 |   
218 | 
219 |   //calculate the error which is cross product between estimated and measured direction of gravity
220 |   halfex = ay*halfvz - az*halfvy;
221 |   halfey = az*halfvx - ax*halfvz;
222 |   halfez = ax*halfvy - ay*halfvx; 
223 | 
224 |   ROS_INFO("The halfex=%f, halfey=%f, halfez=%f", halfex, halfey, halfez);
225 | 
226 |   if (twoKi > 0.0f) {
227 |   //integral the error
228 |   ex_int += twoKi*halfex*dt;
229 |   ey_int += twoKi*halfey*dt;
230 |   ez_int += twoKi*halfez*dt;
231 | 
232 |   ROS_INFO("The ex_int=%f, ey_int=%f, ez_int=%f", ex_int, ey_int, ez_int);
233 | 
234 |   //apply the integral feedback
235 |   gx += ex_int;
236 |   gy += ey_int;
237 |   gz += ez_int;  }
238 | 
239 |   else {
240 |   ex_int = 0.0f;
241 |   ey_int = 0.0f;
242 |   ez_int = 0.0f;
243 |   }
244 | 
245 |   //apply the proportional feedback
246 |   gx += twoKp*halfex;
247 |   gy += twoKp*halfey;
248 |   gz += twoKp*halfez;  
249 |   
250 |   ROS_INFO("The revised gx=%f, gy=%f, gz=%f ", gx, gy, gz);  }
251 | 
252 |   //calculate the rotary angle
253 |   thetax = 0.5*gx*dt;
254 |   thetay = 0.5*gy*dt;
255 |   thetaz = 0.5*gz*dt;
256 | 
257 |   //update quaternion using Runge Kutta algorithms.
258 |   //explanation: http://blog.csdn.net/aasdsadad/article/details/73080832
259 |   dq0 = -q1*thetax-q2*thetay-q3*thetaz;
260 |   dq1 = q0*thetax+q2*thetaz-q3*thetay;
261 |   dq2 = q0*thetay-q1*thetaz+q3*thetax;
262 |   dq3 = q0*thetaz+q1*thetay-q2*thetax;
263 | 
264 |   q0 += dq0;
265 |   q1 += dq1;
266 |   q2 += dq2;
267 |   q3 += dq3;
268 | 
269 |   //normalize quaternion
270 |   recipNorm = invSqrt(q0*q0+q1*q1+q2*q2+q3*q3);
271 |   q0 *= recipNorm;
272 |   q1 *= recipNorm;
273 |   q2 *= recipNorm;
274 |   q3 *= recipNorm; 
275 | 
276 | }
277 | 
278 | 
279 | 


--------------------------------------------------------------------------------
/src/bias_calculator.cpp:
--------------------------------------------------------------------------------
 1 | #include "ros/ros.h"
 2 | #include "geometry_msgs/Vector3.h"
 3 | #include "sensor_msgs/Imu.h"
 4 | 
 5 | double x = 0, y = 0, z = 0;
 6 | double count = 0.0;
 7 | geometry_msgs::Vector3 v;
 8 | 
 9 | void calculator(sensor_msgs::Imu  imu_msg) {
10 |   x += imu_msg.angular_velocity.x;
11 |   y += imu_msg.angular_velocity.y;
12 |   z += imu_msg.angular_velocity.z;
13 |   count++;
14 | }
15 | 
16 | int main(int argc, char** argv) {
17 |   ros::init(argc, argv, "bias_calculator");
18 |   ROS_INFO("Please keep your IMU still for 10 secs");
19 |   ros::NodeHandle n;
20 |   ros::Subscriber sub = n.subscribe("imu0", 1000, calculator);
21 |   ros::Publisher pub = n.advertise< geometry_msgs::Vector3>("bias", 1);
22 | 
23 |   while(ros::ok()) {
24 |     for(int i=0; i<10; i++)
25 |        {ros::Duration(1).sleep();
26 |         ROS_INFO("%ds left", 10-i); 
27 |     ros::spinOnce(); }
28 |     if(count == 0)
29 |        { ROS_WARN("can't receive imu0 data. Please try again.");
30 |         break; }
31 |     v.x = x /count;
32 |     v.y = y /count;
33 |     v.z = z /count;
34 |     pub.publish(v);
35 |     ROS_INFO("Calculate ends."); 
36 |     ROS_INFO("gyro's initial data offest is %f, y is %f, z is %f", v.x, v.y, v.z);
37 |     break;
38 |   }
39 | }
40 |   
41 | 


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