Fixed Kalman filter

src/imu_cleaning.py

@@ -13,8 +13,8 @@ class VariableMaintainer:
         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]
+        self.est = [0, 0, -9.8]
+        self.prev_err_est = [.1, .1, .1]
 
     @property
     def time(self):
@@ -29,15 +29,18 @@ 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]]
+    kg = [vmaint.prev_err_est[0]/(vmaint.prev_err_est[0]+math.sqrt(msg.linear_acceleration_covariance[0])),
+          vmaint.prev_err_est[1]/(vmaint.prev_err_est[1]+math.sqrt(msg.linear_acceleration_covariance[4])),
+          vmaint.prev_err_est[2]/(vmaint.prev_err_est[2]+math.sqrt(msg.linear_acceleration_covariance[8]))]
+    print("Kalman Gain is: ", kg)
+    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]))]
+    print("Current Estimate is: ", curr_est)
+    error_est = [(1 - kg[0]) * vmaint.prev_err_est[0],
+                 (1 - kg[1]) * vmaint.prev_err_est[1],
+                 (1 - kg[2]) * vmaint.prev_err_est[2]]
+    print("Estimate Error is: ", error_est)
     msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z = curr_est
     vmaint.est = curr_est
     vmaint.prev_err_est = error_est
@@ -47,15 +50,6 @@ def publish_clean_imu(msg, vmaint):
 def 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

src/publish_vel_dist.py

@@ -42,7 +42,7 @@ def 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.Subscriber('clean_imu', Imu, publish_velocity_distance, vmaint)
     rospy.spin()