more fixes in kalman filter, creation of r matrix