Fix out of bounds matrix access in tests