More filters

CMakeLists.txt

@@ -6,8 +6,6 @@ project(imu_pipeline)
 find_package(catkin REQUIRED roscpp sensor_msgs geometry_msgs nav_msgs tf)
 
 catkin_package(
-  INCLUDE_DIRS
-  LIBRARIES 
   CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs nav_msgs tf
 )
 
@@ -22,8 +20,24 @@ target_link_libraries(imu_integrator ${catkin_LIBRARIES})
 add_executable(imu_bias_remover src/imu_bias_remover.cpp)
 target_link_libraries(imu_bias_remover ${catkin_LIBRARIES})
 
+add_executable(MadgwickAHRS src/MadgwickAHRS_node.cpp)
+add_library(libMadgwickAHRS src/MadgwickAHRS/MadgwickAHRS.c)
+target_link_libraries(MadgwickAHRS libMadgwickAHRS ${catkin_LIBRARIES})
+
+add_executable(MahonyAHRS src/MahonyAHRS_node.cpp)
+add_library(libMahonyAHRS src/MahonyAHRS/MahonyAHRS.c)
+target_link_libraries(MahonyAHRS libMahonyAHRS ${catkin_LIBRARIES})
+
 # Install targets
-install(TARGETS imu_transformer imu_integrator imu_bias_remover
+install(TARGETS imu_transformer 
+                imu_integrator 
+                imu_bias_remover 
+                MadgwickAHRS 
+                MahonyAHRS
 	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-)
+)
+
+install(PROGRAMS  scripts/*.py
+  DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+)

README.md

@@ -13,4 +13,74 @@ Simple planar imu integration tool.
 imu_bias_remover
 ================
 
-Tool to recalibrate imu bias based on /cmd_vel or /odom
+Tool to recalibrate imu bias based on /cmd_vel or /odom
+
+
+cal_accel_trim.py
+=================
+
+Removes bias from the accelerometer by recording the intial orientation of the
+sensor at startup. This initial orientation is then used as the referance point
+for the gravity vector for downstream filter stages.
+
+
+cal_gyro_bias.py
+=================
+
+Continiously removes gyro biases with an exponential smoothing filter.
+
+
+orient_accel.py
+===============
+
+Generates the orientation of the sensor from only accelerometer data
+
+
+orient_gyro_simple.py
+===============
+
+Generates the orientation of the sensor from only gyro data. 
+The algorithm simply integrates the rates about each axis.
+
+
+orient_gyro_quaternion.py
+=========================
+
+Generates the orientation of the sensor from only gyro data. 
+The algorithm generates a quaternion representation of the current rate 
+measurement (dQ). dQ is then integrated to produce the resulting orientation.
+
+
+orient_accgyr.py
+================
+
+Simple fusion algorithm for combining the accelerometer and gyroscope data to
+form an orientation estimate. Uses a weighted quaternion SLERP for fusion of the
+information sources.
+
+
+orient_tf.py
+============
+
+Publish the orientation from an IMU message as a TF message.
+
+
+MadgwickAHRS & MahonyAHRS
+=========================
+
+ROS wrappers for generating orientation estimates via the open source algorithms
+provided at http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+
+The only change that was made to the upstream implementation is a dynamic update
+rate. The upstream version relies on a fixed update rate.
+
+
+
+
+
+
+
+
+
+
+

launch/orientation_accgyro.launch

@@ -0,0 +1,10 @@
+<launch>
+
+  <node pkg="imu_pipeline" type="cal_gyro_bias.py"  name="ip1" output="screen"/>
+  <node pkg="imu_pipeline" type="cal_accel_trim.py" name="ip2" output="screen"/>
+  <node pkg="imu_pipeline" type="orient_accgyr.py"  name="ip3" output="screen"/>
+  <node pkg="imu_pipeline" type="orient_tf.py"      name="ip4" output="screen"/>
+
+</launch>
+
+

launch/orientation_mag.launch

@@ -0,0 +1,10 @@
+<launch>
+
+  <node pkg="imu_pipeline" type="cal_gyro_bias.py"  name="ip1" output="screen"/>
+  <node pkg="imu_pipeline" type="cal_accel_trim.py" name="ip2" output="screen"/>
+  <node pkg="imu_pipeline" type="orient_mag.py"     name="ip3" output="screen"/>
+  <node pkg="imu_pipeline" type="orient_tf.py"      name="ip4" output="screen"/>
+
+</launch>
+
+

package.xml

@@ -24,4 +24,5 @@
   <run_depend>geometry_msgs</run_depend>
   <run_depend>nav_msgs</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>python_msg_conversions</run_depend>
 </package>

scripts/cal_accel_trim.py

@@ -0,0 +1,136 @@
+#!/usr/bin/env python
+# 
+# Accelerometer Trim Filter - Exponential Moving Averate at startup
+#
+# Copyright (c) 2013 Dereck Wonnacott <[email protected]>
+# 
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+# 
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+# 
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+# 
+
+import rospy
+from sensor_msgs.msg  import Imu
+
+import math
+
+# http://en.wikipedia.org/wiki/Exponential_smoothing
+def update(oldval, newval):
+  alpha = 0.5
+  oldval = alpha * oldval + (1.0 - alpha) * newval
+  return oldval
+
+
+# Filter the messages   
+def sub_imuCB(msg_in): 
+  global pub_imu
+  global trim_x
+  global trim_y
+  global trim_z
+  global prev_x
+  global prev_y
+  global prev_z
+
+  auto_calibration_threshold = 0.001    # Gravity vector
+  g  = 9.8   
+  gx = math.pi / 2
+  gy = math.pi / 2
+  gz = 0
+
+  if(trim_x is None or trim_y is None or trim_z is None):  
+    # Autocalibration finds the average of the three sensor axes
+    # and waits for it to settle before starting the filter. 
+
+    # Sensor vector
+    x = msg_in.linear_acceleration.x 
+    y = msg_in.linear_acceleration.y
+    z = msg_in.linear_acceleration.z
+
+    sr = math.sqrt(z**2 + y**2 + x**2)
+    sx = math.acos(x/sr)
+    sy = math.acos(y/sr)
+    sz = math.acos(z/sr)
+
+    # Update Filter
+    prev_prev_x = prev_x
+    prev_prev_y = prev_y
+    prev_prev_z = prev_z
+
+    prev_x = update(prev_x, sx - gx)
+    prev_y = update(prev_y, sy - gy)
+    prev_z = update(prev_z, sz - gz)
+
+    # Check if the filter has stabilized
+    if (abs(prev_prev_x - prev_x) < auto_calibration_threshold):
+      trim_x = prev_x
+
+    if (abs(prev_prev_y - prev_y) < auto_calibration_threshold):
+      trim_y = prev_y
+
+    if (abs(prev_prev_z - prev_z) < auto_calibration_threshold):
+      trim_z = prev_z
+
+    if(trim_x is not None and trim_y is not None and trim_z is not None):
+      rospy.loginfo("Trim calibration complete.")
+
+  else:
+    # Rotate the sensor vector by the trim vector
+
+    # Sensor vector
+    x = msg_in.linear_acceleration.x 
+    y = msg_in.linear_acceleration.y
+    z = msg_in.linear_acceleration.z
+
+    sr = math.sqrt(z**2 + y**2 + x**2)
+    sx = math.acos(x/sr)
+    sy = math.acos(y/sr)
+    sz = math.acos(z/sr)
+
+    # Output Vector
+    msg_in.linear_acceleration.x = sr * math.cos(sx - trim_x) 
+    msg_in.linear_acceleration.y = sr * math.cos(sy - trim_y)
+    msg_in.linear_acceleration.z = sr * math.cos(sz - trim_z)
+
+  pub_imu.publish(msg_in)
+
+
+if __name__ == '__main__':
+  global pub_imu
+  global trim_x
+  global trim_y
+  global trim_z
+  global prev_x
+  global prev_y
+  global prev_z
+
+  rospy.init_node('imu_trim')
+
+  pub_imu  = rospy.Publisher("/imu/trim", Imu)
+
+  trim_x = rospy.get_param("~trim_x", None)
+  trim_y = rospy.get_param("~trim_y", None)  
+  trim_z = rospy.get_param("~trim_z", None)
+
+  prev_x = 0.0
+  prev_y = 0.0
+  prev_z = 0.0
+
+  rospy.Subscriber("/imu/bias", Imu,  sub_imuCB)
+  rospy.spin()
+
+
+

scripts/cal_gyro_bias.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+# 
+# Gyroscope bias filter - Exponential Moving Average
+#
+# Copyright (c) 2013 Dereck Wonnacott <[email protected]>
+# 
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+# 
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+# 
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+# 
+
+import rospy
+from sensor_msgs.msg  import Imu
+
+
+# http://en.wikipedia.org/wiki/Exponential_smoothing
+def update(oldval, newval):
+  alpha = 0.995
+  oldval = alpha * oldval + (1.0 - alpha) * newval
+  return oldval
+
+
+def sub_imuCB(msg_in): 
+  global pub_imu
+  global prev_x
+  global prev_y
+  global prev_z
+
+  msg_in.angular_velocity.x -= update(prev_x, msg_in.angular_velocity.x)
+  msg_in.angular_velocity.y -= update(prev_y, msg_in.angular_velocity.y)
+  msg_in.angular_velocity.z -= update(prev_z, msg_in.angular_velocity.z)
+
+  pub_imu.publish(msg_in)
+
+
+if __name__ == '__main__':
+  global pub_imu
+  global prev_x
+  global prev_y
+  global prev_z
+
+  rospy.init_node('imu_bias_exp')
+
+  pub_imu  = rospy.Publisher("/imu/bias", Imu)
+
+  prev_x = 0.0
+  prev_y = 0.0
+  prev_z = 0.0
+
+  rospy.Subscriber("/android/imu", Imu,  sub_imuCB)
+  rospy.spin()
+
+
+