Optimized matlab code