From: Erik Andresen <erik@vontaene.de>
Date: Wed, 4 Jul 2018 12:23:17 +0000 (+0200)
Subject: dwm1000:
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=112a7d2803b300557cf90e1cd48c0f81a8b7070e;p=ros_wild_thumper.git

dwm1000:

-skip NaN and values too far away
-kalman optimization
---

diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py
index 69ce1c0..d36a5d2 100755
--- a/scripts/dwm1000.py
+++ b/scripts/dwm1000.py
@@ -47,6 +47,7 @@ class DW1000(threading.Thread):
 		self.dist = 0
 		self.offset = offset
 		self.addr = addr
+		self.name = name
 
 		self.pub = rospy.Publisher(name, Float32, queue_size=16)
 
@@ -62,18 +63,29 @@ class DW1000(threading.Thread):
 		return self.dist
 
 	def run(self):
+		last_val = 10
 		while True:
-			self.dist = self.get_value() + self.offset
-			self.pub.publish(self.distance())
+			val = self.get_value()
+			if abs(val - last_val)  > 10:
+				print "Ignoring values too far apart %s: %.2f - %.2f" % (self.name, val, last_val)
+			elif not isnan(val):
+				self.dist = val + self.offset
+				self.pub.publish(self.distance())
+				last_val = val
 			sleep(0.1)
 
 class Position:
 	def __init__(self):
-		P_est = 0.003 # Fehlerkovarianz
-		Q = 10e-4 # Systemrauschen
-		R = 0.1 # Varianz des Messfehlers
-		self.filter_x = simple_kalman(1.0, P_est, Q, R)
-		self.filter_y = simple_kalman(0.0, P_est, Q, R)
+		# Varianz des Messfehlers
+		Rx = 0.2
+		Ry = 0.05
+		# Fehlerkovarianz
+		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)
 		self.speed_x = 0
 		self.speed_y = 0
 		self.speed_z = 0