Kalman.cpp completed, filter still needs testing