optimize matlab code