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

diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py
index 15c4025..52a9e27 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)
@@ -125,14 +129,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
 			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.017795,  -0.021832, 0.010968], dist))
+			self.filter_y.set_measure_cov(np.polyval([0.0060314, -0.013387, 0.0065049], dist))
 		else:
 			x = self.filter_x.x_est
 			y = self.filter_y.x_est
@@ -162,7 +165,7 @@ if __name__ == "__main__":
 		dist_left = dwleft.distance_valid()
 		dist_right = dwright.distance_valid()
 		if dist_left == None or dist_right == None:
-			rospy.logerr("no valid sensor update")
+			rospy.logerr_throttle(10, "no valid sensor update")
 			# run kalman prediction only
 			pos.filter(None, None)
 		else: