Fixed Kalman library issues

Fixed Kalman library issues and made Kalman usage optional