Kalman iteration complete

+ some minor functions to matrixLib