To run: octave main Requires GNUPlot Output: Red: EKF belief Blue: Ground Truth Green: Odometry only belief