+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
+ x = self.filter_x.run(x)
+ y = self.filter_y.run(y)
+
+ self.last_time = current_time
+ return x,y
+
+