+class Position:
+ def __init__(self):
+ P_est = 0.003 # Fehlerkovarianz
+ Q = 10e-4 # Systemrauschen
+ R = 0.1 # Varianz des Messfehlers
+ self.filter_x = simple_kalman(1.0, P_est, Q, R)
+ self.filter_y = simple_kalman(0.0, 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 = 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
+
+