Small chnges made while testing Kalmanfilter.