Soft

First code
Add kalman filter