commit before re-basing computer. still some bugs in the Kalman Filter -Daman