wrote some tests. Kalman structure working. Needs to be thoroughly tested