X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fdwm1000.py;h=707dff19f05e75865b6371b789daf84a1159db86;hp=15c4025350c673981382ecd76385ed17fe75cb93;hb=f04118a96832b33d160805b1e07e738a41102b84;hpb=721db3c859a69179a9f956a53ae7e40cf0ca655b

diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py
index 15c4025..707dff1 100755
--- a/scripts/dwm1000.py
+++ b/scripts/dwm1000.py
@@ -42,6 +42,10 @@ class simple_kalman:
 
 		return x
 
+	def set_measure_cov(self, R):
+		if R > 0:
+			self.R = R
+
 class DW1000(threading.Thread):
 	def __init__(self, name, addr, offset):
 		threading.Thread.__init__(self)
@@ -75,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
@@ -93,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
@@ -125,14 +132,13 @@ class Position:
 
 		# run kalman if new measurements are valid
 		if x != None and y != None:
-			print "Var", self.filter_x.R, self.filter_y.R
 			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.R = np.polyval([0.017795,  -0.021832, 0.010968], dist)
-			self.filter_y.R = 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
@@ -150,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()
@@ -161,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("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"
 
@@ -190,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)
@@ -202,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()