Matlab Control Algorithm