From f04118a96832b33d160805b1e07e738a41102b84 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Fri, 27 Jul 2018 09:03:28 +0200 Subject: [PATCH 1/1] dwm1000: Always send transform even if there are no valid sensors values --- scripts/dwm1000.py | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py index 52a9e27..707dff1 100755 --- a/scripts/dwm1000.py +++ b/scripts/dwm1000.py @@ -79,7 +79,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 @@ -97,9 +97,12 @@ class Position: 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) + Qx = 0.001 + Qy = 0.00001 + # TODO: Replace 2x 1d kalman with 2d kalman + # so the polyfit variance can be replaced with a covariance matrix + self.filter_x = simple_kalman(1.0, P_est_x, Qx, Rx) + self.filter_y = simple_kalman(0.0, P_est_y, Qy, Ry) self.speed_x = 0 self.speed_y = 0 self.speed_z = 0 @@ -132,10 +135,10 @@ class Position: x = self.filter_x.run(x) y = self.filter_y.run(y) - # 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)) + self.filter_x.set_measure_cov(np.polyval([0.018, 0.0, 0.0], dist)) + self.filter_y.set_measure_cov(np.polyval([0.006, 0.0, 0.0], dist)) else: x = self.filter_x.x_est y = self.filter_y.x_est @@ -153,8 +156,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.0) + dwright = DW1000("uwb_dist_right", 0xc0, -0.0) dist_l_r = 0.285 # Distance between both DWM1000 rate = rospy.Rate(10) pos = Position() @@ -164,10 +167,12 @@ 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" @@ -193,7 +198,6 @@ 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: circle_left = plt.Circle((-dist_l_r/2, 0), dwleft.distance, color='red', fill=False) @@ -205,6 +209,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() -- 2.39.2