Updated to use Kalman filter

src/imu_cleaning.py

@@ -2,43 +2,66 @@
 import rospy
 from geometry_msgs.msg import Quaternion, Vector3
 from sensor_msgs.msg import Imu
-from std_msgs.msg import *
+from std_msgs.msg import Header
 
 import math
 
-class VariableMaintainer():
-	def __init__(self):
-		self._time = rospy.Time.now()
-
-	@property
-	def time(self):
-		return self._time
-
-	@time.setter
-	def time(self, value):
-		self._time = value
-
-def publishCleanImu(msg, correctionVector):
-	pub = rospy.Publisher('clean_imu',Imu)
-	msg.linear_acceleration.x -= correctionVector[0]
-	msg.linear_acceleration.y -= correctionVector[1]
-	msg.linear_acceleration.y -= correctionVector[2]
-	pub.publish(msg)
+
+class VariableMaintainer:
+    def __init__(self):
+        self._time = rospy.Time.now()
+        self.lin_x = None
+        self.lin_y = None
+        self.lin_z = None
+        self.est = [0,0,-9.8]
+        self.prev_err_est = [1, 1, 1]
+
+    @property
+    def time(self):
+        return self._time
+
+    @time.setter
+    def time(self, value):
+        self._time = value
+
+
+def publish_clean_imu(msg, vmaint):
+    pub = rospy.Publisher('clean_imu', Imu)
+    # Implement Kalman Filter
+    # Error in measurement comes from covariance matrix. Only variances are provided by this IMU(Razor9DOF)
+    kg = [vmaint.prev_err_est[0]/(vmaint.prev_err_est[0]+msg.linear_acceleration_covariance[0]),
+          vmaint.prev_err_est[1]/(vmaint.prev_err_est[1]+msg.linear_acceleration_covariance[4]),
+          vmaint.prev_err_est[2]/(vmaint.prev_err_est[2]+msg.linear_acceleration_covariance[8])]
+    curr_est = [vmaint.est[0] + (kg[0] * (msg.linear_acceleration.x + vmaint.est[0])),
+                vmaint.est[1] + (kg[1] * (msg.linear_acceleration.y + vmaint.est[1])),
+                vmaint.est[2] + (kg[2] * (msg.linear_acceleration.z + vmaint.est[2]))]
+    error_est = [(1 - kg[0]) * vmaint.est[0],
+                 (1 - kg[0]) * vmaint.est[0],
+                 (1 - kg[0]) * vmaint.est[0]]
+    msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z = curr_est
+    vmaint.est = curr_est
+    vmaint.prev_err_est = error_est
+    pub.publish(msg)
+
 
 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)))
-	rospy.Subscriber('imu', Imu, publishCleanImu, averages)
-	rospy.spin()
-
-if __name__=='__main__':
-	main()
+    # Collect first 50 entries and average them.
+    rospy.init_node("imu_cleaner")
+    totals = [0, 0, 0]
+    for i in range(50):
+        c_val = rospy.client.wait_for_message('imu', Imu)
+        totals[0] += c_val.linear_acceleration.x
+        totals[1] += c_val.linear_acceleration.y
+        totals[2] += c_val.linear_acceleration.z
+    averages = map(lambda x: x / 50, totals)
+    print("These are the average values over the first 50 records", averages)
+    print("Magnitude is:", math.sqrt(math.pow(averages[0], 2) + math.pow(averages[1], 2) + math.pow(averages[2], 2)))
+    vmaint = VariableMaintainer()
+    c_val = rospy.client.wait_for_message('imu', Imu)
+    vmaint.lin_x, vmaint.lin_y, vmaint.lin_z = c_val.linear_acceleration.x, c_val.linear_acceleration.y, c_val.linear_acceleration.z
+    rospy.Subscriber('imu', Imu, publish_clean_imu, vmaint)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()

src/publish_vel_dist.py

@@ -6,41 +6,45 @@ from std_msgs.msg import *
 
 import math
 
-class VariableMaintainer():
-	def __init__(self):
-		self.time = False
-        	self.velocity = [0,0,0]
-        	self.position = [0,0,0]
-
-def publishVelDist(msg, vmaint):
-    velPub = rospy.Publisher('calc_velocity', Vector3)
-    posPub = rospy.Publisher('calc_position', Vector3)
-    if not vmaint.time:     # This means this is the first run
-        vmaint.time = msg.header.stamp.secs + msg.header.stamp.nsecs*math.pow(10,-9)
-        vmaint.velocity = [0,0,0]
-        vmaint.position = [0,0,0]
+
+class VariableMaintainer:
+    def __init__(self):
+        self.time = False
+        self.velocity = [0, 0, 0]
+        self.position = [0, 0, 0]
+
+
+def publish_velocity_distance(msg, vmaint):
+    vel_pub = rospy.Publisher('calc_velocity', Vector3)
+    pos_pub = rospy.Publisher('calc_position', Vector3)
+    if not vmaint.time:  # This means this is the first run
+        vmaint.time = msg.header.stamp.secs + msg.header.stamp.nsecs * math.pow(10, -9)
+        vmaint.velocity = [0, 0, 0]
+        vmaint.position = [0, 0, 0]
     else:
-        ctime = msg.header.stamp.secs + msg.header.stamp.nsecs*math.pow(10,-9)
-        tdelt = ctime - vmaint.time
+        c_time = msg.header.stamp.secs + msg.header.stamp.nsecs * math.pow(10, -9)
+        tdelt = c_time - vmaint.time
         accel = [msg.linear_acceleration.x,
                  msg.linear_acceleration.y,
                  msg.linear_acceleration.z]
-        map(lambda x: x*tdelt, accel)
-        vmaint.velocity = [vmaint.velocity[i]+accel[i] for i in range(3)]
-        map(lambda x: x*tdelt, accel)
-        vmaint.velocity = [vmaint.velocity[i]+accel[i] for i in range(3)]
-        vmaint.time = ctime
-        v = Vector3(vmaint.velocity[0],vmaint.velocity[1],vmaint.velocity[2])
-        p = Vector3(vmaint.position[0],vmaint.position[1],vmaint.position[2])
-        velPub.publish(v)
-        posPub.publish(p)
+        map(lambda x: x * tdelt, accel)
+        vmaint.velocity = [vmaint.velocity[i] + accel[i] for i in range(3)]
+        map(lambda x: x * tdelt, accel)
+        vmaint.position = [vmaint.velocity[i] + accel[i] for i in range(3)]
+        vmaint.time = c_time
+        v = Vector3(vmaint.velocity[0], vmaint.velocity[1], vmaint.velocity[2])
+        p = Vector3(vmaint.position[0], vmaint.position[1], vmaint.position[2])
+        vel_pub.publish(v)
+        pos_pub.publish(p)
+
 
 def main():
-	# Collect first 50 entries and average them.
-	rospy.init_node("publish_vel_dist")
-    	vmaint = VariableMaintainer()
-	rospy.Subscriber('imu', Imu, publishVelDist, vmaint)
-	rospy.spin()
-
-if __name__=='__main__':
-	main()
+    # Collect first 50 entries and average them.
+    rospy.init_node("publish_vel_dist")
+    vmaint = VariableMaintainer()
+    rospy.Subscriber('imu', Imu, publish_velocity_distance, vmaint)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()