Fixed bugs with residual
Added correct penalty function