Kalman functions

old kalman filter functions … tried to find most recent versions, but
some may not be working