Draw trajectory

Change-Id: I98e8cf0d57174da4e5aea6c9947d0dc2062b01b2