Extracted parameters into a roslaunch capable interface
Additional Error checking
Added the EKF debug message
Added a demo launch file for 5-state EKF
Need to tune parameters!