kalman filter improved

I just realized that the kalman filter that I had implemented had some minor glitches so removed them.

Previous Kalman filter -> http://www.youtube.com/watch?v=ZGhGeKQMyVA
Improved Kalman filter -> http://www.youtube.com/watch?v=ZPAGDolFTEg