Filtering Path Plan using Kalman Filter and simplify it to N Points ROS