Added a check in the plot trajectory function to allow you to pass in 4x4 transformation matrices