X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fdwm1000.py;h=b16ff9ecb30faca5ef2727d391bb15e7866da337;hp=52a9e2730bc9853bfbf80efaaa42342fcb8ff6cb;hb=2f194b3fe82009a77b79021f0c57e6ca55c1706f;hpb=3e4ac9f32b7c6b765650b9bae905dd0d81337f57 diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py index 52a9e27..b16ff9e 100755 --- a/scripts/dwm1000.py +++ b/scripts/dwm1000.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # -*- coding: iso-8859-15 -*- -VISULAIZE = False +VISUALIZE = False import threading import struct @@ -15,36 +15,10 @@ from time import sleep from std_msgs.msg import Float32 from nav_msgs.msg import Odometry from wild_thumper.srv import DWM1000Center, DWM1000CenterResponse -if VISULAIZE: +from pyshared.simple_kalman import simple_kalman +if VISUALIZE: import matplotlib.pyplot as plt -class simple_kalman: - def __init__(self, x_est, P_est, Q, R): - self.x_est = x_est # Systemzustand - self.P_est = P_est # Fehlerkovarianz - self.Q = Q # Systemrauschen - self.R = R # Varianz des Messfehlers - - def run(self, y): - # Korrektur mit der Messung - # (1) Berechnung der Kalman Verstärkung - K = self.P_est/(self.R + self.P_est) - # (2) Korrektur der Schätzung mit der Messung y - x = self.x_est + K*(y - self.x_est) - # (3) Korrektur der Fehlerkovarianzmatrix - P = (1-K)*self.P_est - # - # Prädiktion - # (1) Prädiziere den Systemzustand - self.x_est = x - # (2) Präzidiere die Fehlerkovarianzmatrix - self.P_est = P + self.Q - - return x - - def set_measure_cov(self, R): - if R > 0: - self.R = R class DW1000(threading.Thread): def __init__(self, name, addr, offset): @@ -79,7 +53,7 @@ class DW1000(threading.Thread): last_val = 10 while not rospy.is_shutdown(): val = self.get_value() - if abs(val - last_val) > 10: + if abs(val - last_val) > 50: rospy.logwarn("Ignoring values too far apart %s: %.2f - %.2f", self.name, val, last_val) elif not isnan(val): self.dist = val + self.offset @@ -90,16 +64,10 @@ class DW1000(threading.Thread): class Position: def __init__(self): - # Varianz des Messfehlers - Rx = 0.2 - Ry = 0.05 - # Fehlerkovarianz - P_est_x = 0.02 - P_est_y = 0.01 - # Systemrauschen - Q = 0.002 - self.filter_x = simple_kalman(1.0, P_est_x, Q, Rx) - self.filter_y = simple_kalman(0.0, P_est_y, Q, Ry) + R = np.matrix([[0.5, 0.0], [0.0, 0.2]]) # Varianz des Messfehlers + P_est = 1e50 * np.eye(2) # Fehlerkovarianz + Q = np.matrix([[0.001, 0.0], [0.0, 0.0001]]) # Systemrauschen + self.filter_xy = simple_kalman(np.array([[1.0, 0.0]]).T, P_est, Q, R) self.speed_x = 0 self.speed_y = 0 self.speed_z = 0 @@ -116,7 +84,7 @@ class Position: current_time = rospy.Time.now() dt = (current_time - self.last_time).to_sec() # Subtract vehicle speed - pos = np.array([self.filter_x.x_est, self.filter_y.x_est]) + pos = self.filter_xy.x_est.T # translation pos -= np.array([self.speed_x*dt, self.speed_y*dt]) # rotation @@ -124,21 +92,25 @@ class Position: [np.sin(self.speed_z*dt), np.cos(self.speed_z*dt)]]) pos = np.dot(pos, rot) # copy back - self.filter_x.x_est = pos[0] - self.filter_y.x_est = pos[1] + self.filter_xy.x_est = pos.T # run kalman if new measurements are valid + pos = None if x != None and y != None: - x = self.filter_x.run(x) - y = self.filter_y.run(y) + pos = self.filter_xy.run(np.array([[x, y]]).T) - # Update covariance + # Update variance dist = np.linalg.norm([x, y]) - self.filter_x.set_measure_cov(np.polyval([0.017795, -0.021832, 0.010968], dist)) - self.filter_y.set_measure_cov(np.polyval([0.0060314, -0.013387, 0.0065049], dist)) + cov_00 = np.polyval([0.018, 0.0, 0.0], dist) + cov_11 = np.polyval([0.006, 0.0, 0.0], dist) + self.filter_xy.set_measure_cov(np.matrix([[cov_00, 0.0], [0.0, cov_11]])) else: - x = self.filter_x.x_est - y = self.filter_y.x_est + pos = self.filter_xy.x_est + self.filter_xy.P_est = np.matrix(1e50 * np.eye(2)) # reset estimate when lost + + + x = pos.item((0, 0)) + y = pos.item((1, 0)) self.last_time = current_time return x,y @@ -153,8 +125,8 @@ def handle_center_call(req): if __name__ == "__main__": rospy.init_node('DWM1000', log_level=rospy.DEBUG) - dwleft = DW1000("uwb_dist_left", 0xc2, +0.00) - dwright = DW1000("uwb_dist_right", 0xc0, -0.00) + dwleft = DW1000("uwb_dist_left", 0xc2, -0.04) + dwright = DW1000("uwb_dist_right", 0xc0, +0.04) dist_l_r = 0.285 # Distance between both DWM1000 rate = rospy.Rate(10) pos = Position() @@ -164,16 +136,18 @@ if __name__ == "__main__": while not rospy.is_shutdown() and dwleft.is_alive() and dwright.is_alive(): dist_left = dwleft.distance_valid() dist_right = dwright.distance_valid() + x = None + y = None if dist_left == None or dist_right == None: - rospy.logerr_throttle(10, "no valid sensor update") + rospy.logerr_throttle(10, "no valid sensor update %r %r" % (dist_left, dist_right)) # run kalman prediction only - pos.filter(None, None) + x,y = pos.filter(None, None) else: dir = "left" if (dist_left < dist_right) else "right" diff = abs(dist_left - dist_right) if diff >= dist_l_r: - # difference to high, correct to maximum + # difference too high, correct to maximum off = diff - dist_l_r + 0.01 if dist_left > dist_right: dist_left -= off/2 @@ -193,9 +167,8 @@ if __name__ == "__main__": x, y = (y, -x) x, y = pos.filter(x, y) - tf_broadcaster.sendTransform((x, y, 0.0), (0, 0, 0, 1), rospy.Time.now(), "uwb_beacon", "base_footprint") - if VISULAIZE: + if VISUALIZE: circle_left = plt.Circle((-dist_l_r/2, 0), dwleft.distance, color='red', fill=False) circle_right = plt.Circle((dist_l_r/2, 0), dwright.distance, color='green', fill=False) plt.gca().add_patch(circle_left) @@ -205,6 +178,7 @@ if __name__ == "__main__": plt.show() else: # No current position, still need up update kalman prediction - pos.filter(None, None) + x, y = pos.filter(None, None) + tf_broadcaster.sendTransform((x, y, 0.0), (0, 0, 0, 1), rospy.Time.now(), "uwb_beacon", "base_footprint") rate.sleep()