'working on TODO'
changed kalman.c to ekf_tester.c