Finished basic kalman filter and test.