+class Position:
+ def __init__(self):
+ # Varianz des Messfehlers
+ Rx = 0.2
+ Ry = 0.05
+ # Fehlerkovarianz
+ P_est_x = 0.02
+ P_est_y = 0.01
+ # Systemrauschen
+ Q = 0.002
+ self.filter_x = simple_kalman(1.0, P_est_x, Q, Rx)
+ self.filter_y = simple_kalman(0.0, P_est_y, Q, Ry)
+ 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 = np.array([self.filter_x.x_est, self.filter_y.x_est])
+ # 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_x.x_est = pos[0]
+ self.filter_y.x_est = pos[1]
+
+ # run kalman if new measurements are valid
+ 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
+
+ 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()
+