Working decently, work needs to be done on Kalman Filter