Kalman Filter

This is an initial test file, currently it doesn't work exactly as expected.