Moved base code into a library to simplify unit test creation
Moved kalman filter unit test into its own file
Fixed optimization bug for Kalman filter