Sonar algorithm cleaned & BLE Factor param added

- Forward sonar algorithm cleaned
-SNR_BLE_FACTOR added to control Brown Linear Expo Low Pass Filter