dwm1000 kalman: Check covariance < 0

[ros_wild_thumper.git] / scripts / dwm1000.py

diff --git a/scripts/dwm1000.py b/scripts/dwm1000.py
index bf58657c90a129dbe9579f7475f9b2db9af91a64..52a9e2730bc9853bfbf80efaaa42342fcb8ff6cb 100755 (executable)
--- a/scripts/dwm1000.py
+++ b/scripts/dwm1000.py
@@ -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)


@@ -73,10 +77,10 @@ class DW1000(threading.Thread):
 
        def run(self):
                last_val = 10
 
        def run(self):
                last_val = 10

-               while True:
+               while not rospy.is_shutdown():

                        val = self.get_value()
                        if abs(val - last_val)  > 10:
                        val = self.get_value()
                        if abs(val - last_val)  > 10:

-                               print "Ignoring values too far apart %s: %.2f - %.2f" % (self.name, val, last_val)
+                               rospy.logwarn("Ignoring values too far apart %s: %.2f - %.2f", self.name, val, last_val)

                        elif not isnan(val):
                                self.dist = val + self.offset
                                self.last_update = datetime.now()
                        elif not isnan(val):
                                self.dist = val + self.offset
                                self.last_update = datetime.now()


@@ -107,10 +111,6 @@ class Position:
                self.speed_y = msg.twist.twist.linear.y
                self.speed_z = msg.twist.twist.angular.z
 
                self.speed_y = msg.twist.twist.linear.y
                self.speed_z = msg.twist.twist.angular.z
 

-       """
-       TODO:
-       - variance of kalman should be dependant on distance
-       """

        def filter(self, x, y):
                # Correct estimation with speed
                current_time = rospy.Time.now()
        def filter(self, x, y):
                # Correct estimation with speed
                current_time = rospy.Time.now()


@@ -131,6 +131,11 @@ class Position:
                if x != None and y != None:
                        x = self.filter_x.run(x)
                        y = self.filter_y.run(y)
                if x != None and y != None:
                        x = self.filter_x.run(x)
                        y = self.filter_y.run(y)

+
+                       # Update covariance
+                       dist = np.linalg.norm([x, y])
+                       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


@@ -143,14 +148,14 @@ def handle_center_call(req):
        diff = dwleft.distance_valid() - dwright.distance_valid()
        dwleft.offset -= diff/2
        dwright.offset += diff/2
        diff = dwleft.distance_valid() - dwright.distance_valid()
        dwleft.offset -= diff/2
        dwright.offset += diff/2

-       print "Centering to %.2f %.2f" % (dwleft.offset, dwright.offset)
+       rospy.loginfo("Centering to %.2f %.2f", dwleft.offset, dwright.offset)

        return DWM1000CenterResponse()
 
 if __name__ == "__main__":
        return DWM1000CenterResponse()
 
 if __name__ == "__main__":

-       rospy.init_node('DWM1000')
-       dwleft  = DW1000("uwb_dist_left",  0xc2, +0.02)
-       dwright = DW1000("uwb_dist_right", 0xc0, -0.02)
-       dist_l_r = 0.285
+       rospy.init_node('DWM1000', log_level=rospy.DEBUG)
+       dwleft  = DW1000("uwb_dist_left",  0xc2, +0.00)
+       dwright = DW1000("uwb_dist_right", 0xc0, -0.00)
+       dist_l_r = 0.285 # Distance between both DWM1000

        rate = rospy.Rate(10)
        pos = Position()
        tf_broadcaster = tf.broadcaster.TransformBroadcaster()
        rate = rospy.Rate(10)
        pos = Position()
        tf_broadcaster = tf.broadcaster.TransformBroadcaster()


@@ -160,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:

-                       print "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:


@@ -176,14 +181,14 @@ if __name__ == "__main__":
                                else:
                                        dist_left += off/2
                                        dist_right -= off/2
                                else:
                                        dist_left += off/2
                                        dist_right -= off/2

-                       print "%.2f %.2f %.2f %.2f %s" % (dwleft.distance(), dwright.distance(), dist_left, dist_right, dir)
+                       rospy.logdebug("%.2f %.2f %.2f %.2f %s", dwleft.distance(), dwright.distance(), dist_left, dist_right, dir)

 
                        a_r = (-dist_right**2 + dist_left**2 - dist_l_r**2) / (-2*dist_l_r)
                        x = dist_l_r/2 - a_r
                        t = dist_right**2 - a_r**2
                        if t >= 0:
                                y = sqrt(t)
 
                        a_r = (-dist_right**2 + dist_left**2 - dist_l_r**2) / (-2*dist_l_r)
                        x = dist_l_r/2 - a_r
                        t = dist_right**2 - a_r**2
                        if t >= 0:
                                y = sqrt(t)

-                               print x,y
+                               rospy.logdebug("x=%.2f, y=%.2f", x, y)

                                # Rotate 90 deg
                                x, y = (y, -x)
 
                                # Rotate 90 deg
                                x, y = (y, -x)