Clean up source
[ADD] KalmanFilter and Hysterisis