Sonar_vel and Sonar_accel functions added
Add functions to cal Sonar_vel and Sonar_accel
Add new plot in configurator