Created Kalman Filter code, test example, and part way through documentation