From 018b8166b6a2dc13cb53c04a64c985372ea71cfb Mon Sep 17 00:00:00 2001 From: Renukanandan Tumu Date: Fri, 23 Feb 2018 16:57:40 -0500 Subject: [PATCH] Updated to use Kalman filter --- src/imu_cleaning.py | 93 +++++++++++++++++++++++++---------------- src/publish_vel_dist.py | 68 ++++++++++++++++-------------- 2 files changed, 94 insertions(+), 67 deletions(-) diff --git a/src/imu_cleaning.py b/src/imu_cleaning.py index 64757af..a3889dc 100755 --- a/src/imu_cleaning.py +++ b/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() diff --git a/src/publish_vel_dist.py b/src/publish_vel_dist.py index d6f58bf..1960632 100644 --- a/src/publish_vel_dist.py +++ b/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()