dwm1000 kalman: Check covariance < 0
authorErik Andresen <erik@vontaene.de>
Sun, 15 Jul 2018 09:59:02 +0000 (11:59 +0200)
committerErik Andresen <erik@vontaene.de>
Sun, 15 Jul 2018 09:59:02 +0000 (11:59 +0200)
scripts/dwm1000.py
scripts/wt_node.py

index 15c4025..52a9e27 100755 (executable)
@@ -42,6 +42,10 @@ class simple_kalman:
 
                return x
 
 
                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)
 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:
 
                # 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])
                        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
                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:
                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:
                        # run kalman prediction only
                        pos.filter(None, None)
                else:
index 6006f95..f7b02f5 100755 (executable)
@@ -238,7 +238,7 @@ class WTBase:
                        rospy.logerr("Voltage critical: %.2fV" % (volt))
                        self.volt_last_warn = rospy.Time.now()
 
                        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):
 
 
        def get_odom(self):