kalman filtering seems to be done, need to test
Merge remote-tracking branch 'origin/master'