simple kalman code implemented