author Erik Andresen Fri, 27 Jul 2018 07:03:28 +0000 (09:03 +0200) committer Erik Andresen Fri, 27 Jul 2018 07:03:28 +0000 (09:03 +0200)

index 52a9e2730bc9853bfbf80efaaa42342fcb8ff6cb..707dff19f05e75865b6371b789daf84a1159db86 100755 (executable)
last_val = 10
while not rospy.is_shutdown():
val = self.get_value()
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
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
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
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)

x = self.filter_x.run(x)
y = self.filter_y.run(y)

-                       # Update covariance
+                       # Update variance
dist = np.linalg.norm([x, y])
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
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)

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()
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()
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:
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
# run kalman prediction only
-                       pos.filter(None, None)
+                       x,y = pos.filter(None, None)
else:
dir = "left" if (dist_left < dist_right) else "right"

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)
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)

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
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()

rate.sleep()