State Estimator further fleshed out. Also a filter is put on the sonars.