From: Erik Andresen <erik@vontaene.de>
Date: Sun, 15 Jul 2018 09:59:02 +0000 (+0200)
Subject: dwm1000 kalman: Check covariance < 0
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=3e4ac9f32b7c6b765650b9bae905dd0d81337f57;p=ros_wild_thumper.git

dwm1000 kalman: Check covariance < 0
---

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:
diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 6006f95..f7b02f5 100755
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -238,7 +238,7 @@ class WTBase:
 			rospy.logerr("Voltage critical: %.2fV" % (volt))
 			self.volt_last_warn = rospy.Time.now()
 
-		self.bDocked = volt > 10
+		self.bDocked = volt > 10.1
 
 
 	def get_odom(self):