Kalman integration

First successful tests with kalman and GPS code