dwm1000:
[ros_wild_thumper.git] / scripts / dwm1000.py
index 69ce1c0..d36a5d2 100755 (executable)
@@ -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