kalman filter implementation how I used many times. Is starting point for kalman filter library