dwm1000 kalman: Check covariance < 0
[ros_wild_thumper.git] / scripts / dwm1000.py
index 15c4025..52a9e27 100755 (executable)
@@ -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: