From 112a7d2803b300557cf90e1cd48c0f81a8b7070e Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Wed, 4 Jul 2018 14:23:17 +0200 Subject: [PATCH] dwm1000: -skip NaN and values too far away -kalman optimization --- scripts/dwm1000.py | 26 +++++++++++++++++++------- 1 file changed, 19 insertions(+), 7 deletions(-) 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 -- 2.39.5