Added smoothing process. Need to work on processing with GPS. Then should
add processing with additional sensors.
Added IMU bias estimaton. Made a robot playback movie feature.
Fixed a bug with the IMU processing. Forward Filter and Smoothing works for the LIDAR Data
Added additional plotting stuff to the movie plot
Cleaned up some of the EKF initialization. GPS/IMU works for a dataset where GPS is offset from origin. This is good