penalty function

Computes penalty based on quadratic form