Added Matlab dive controller testing