+class Position:
+ def __init__(self):
+ R = np.matrix([[0.5, 0.0], [0.0, 0.2]]) # Varianz des Messfehlers
+ P_est = 1e50 * np.eye(2) # Fehlerkovarianz
+ Q = np.matrix([[0.001, 0.0], [0.0, 0.0001]]) # Systemrauschen
+ self.filter_xy = simple_kalman(np.array([[1.0, 0.0]]).T, P_est, Q, R)
+ self.speed_x = 0
+ self.speed_y = 0
+ self.speed_z = 0
+ self.last_time = rospy.Time.now()
+ rospy.Subscriber("/odom_combined", Odometry, self.odomReceived)
+
+ def odomReceived(self, msg):
+ self.speed_x = msg.twist.twist.linear.x
+ self.speed_y = msg.twist.twist.linear.y
+ self.speed_z = msg.twist.twist.angular.z
+
+ def filter(self, x, y):
+ # Correct estimation with speed
+ current_time = rospy.Time.now()
+ dt = (current_time - self.last_time).to_sec()
+ # Subtract vehicle speed
+ pos = self.filter_xy.x_est.T
+ # translation
+ pos -= np.array([self.speed_x*dt, self.speed_y*dt])
+ # rotation
+ rot = np.array([[np.cos(self.speed_z*dt), -np.sin(self.speed_z*dt)],
+ [np.sin(self.speed_z*dt), np.cos(self.speed_z*dt)]])
+ pos = np.dot(pos, rot)
+ # copy back
+ self.filter_xy.x_est = pos.T
+
+ # run kalman if new measurements are valid
+ pos = None
+ if x != None and y != None:
+ pos = self.filter_xy.run(np.array([[x, y]]).T)
+
+ # Update variance
+ dist = np.linalg.norm([x, y])
+ cov_00 = np.polyval([0.018, 0.0, 0.0], dist)
+ cov_11 = np.polyval([0.006, 0.0, 0.0], dist)
+ self.filter_xy.set_measure_cov(np.matrix([[cov_00, 0.0], [0.0, cov_11]]))
+ else:
+ pos = self.filter_xy.x_est
+ self.filter_xy.P_est = np.matrix(1e50 * np.eye(2)) # reset estimate when lost
+
+
+ x = pos.item((0, 0))
+ y = pos.item((1, 0))
+
+ self.last_time = current_time
+ return x,y
+
+
+def handle_center_call(req):
+ diff = dwleft.distance_valid() - dwright.distance_valid()
+ dwleft.offset -= diff/2
+ dwright.offset += diff/2
+ rospy.loginfo("Centering to %.2f %.2f", dwleft.offset, dwright.offset)
+ return DWM1000CenterResponse()
+