Implementation algorithm in Simulink