Updated calculating coordinates + matlab visualization