Added function to smooth trajectory

Added function to smooth trajectory and GUI support