dwm1000 kalman: Check covariance < 0

diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py
index 15c4025350c673981382ecd76385ed17fe75cb93..52a9e2730bc9853bfbf80efaaa42342fcb8ff6cb 100755 (executable)
--- 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 6006f9562bef641e510005c84676553ac6c52801..f7b02f5b009626e9a6dc90544705ed9c1287fd08 100755 (executable)
--- 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):