Function to compute adjusted penalty when orthogonalizing
orthog_P(P, Z)
P
matrix; original penalty matrix
Z
matrix; constraint matrix
adjusted penalty matrix