Kalman filter updates

finished (?)  MD_kalman implementation