dwm1000:

-skip NaN and values too far away
-kalman optimization

diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py
index 69ce1c09bd493d0cb6cd7e4887fe364c59c07a25..d36a5d2f7e683fbc9e96e773ac874aa91821c2b9 100755 (executable)
--- 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