Added matrix-vector multiplication to algorithm