Subscribed to x_hat (published by the kalman filter)

Also wrote a temporary function to fill in the x_command struct. This will be replaced when the path follower is responsible for publishing this struct.