imu_cleaning.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Quaternion, Vector3
from sensor_msgs.msg import Imu

import math

def main():
	# Collect first 50 entries and average them.
	rospy.init_node("imu_cleaner")
	totals =[0,0,0] 
	for i in range(50):
		cval = rospy.client.wait_for_message('imu',Imu)
		totals[0]+=cval.linear_acceleration.x
		totals[1]+=cval.linear_acceleration.y
		totals[2]+=cval.linear_acceleration.z
	averages = map(lambda x: x/50,totals)
	print(averages)
	print("Magnitude is:", math.sqrt(math.pow(averages[0],2)+math.pow(averages[1],2)+math.pow(averages[2],2)))

if __name__=='__main__':
	main()