Integrated Kalman filter into mainFunc (untested obviously but it compiles)