Problem with 1st order Massberg LPF
Problem solved. 

Looks like calculation of Os ($\omega_s$) was the culprit for this problem. Proper equation is:

Os=Om*sqrt((gm^2-g1^2)*(1-gm^2))/(1-gm^2);


instead of:

Os=Om*(sqrt((gm^2-g1^2)*(1-gm^2))/(1-gm^2));