Optimize Kalman Filter, Add documentation