Everything works! Matlab visualization included