Updated kalman constructor and kalmantest