Added matrix rotation

Issues trying to control the pitch, roll and yaw to achieve the
desired result. Started adding an axis indicator to the output