Initial commit for context manager

Implemented Kalman filter
Initial testing ongoing.