projects
/
ros_wild_thumper.git
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
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.0
2
)
- dwright = DW1000("uwb_dist_right", 0xc0, -0.0
2
)
- dist_l_r = 0.285
+ rospy.init_node('DWM1000'
, log_level=rospy.DEBUG
)
+ dwleft = DW1000("uwb_dist_left", 0xc2, +0.0
0
)
+ dwright = DW1000("uwb_dist_right", 0xc0, -0.0
0
)
+ 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)