Take average of diagonal elements

To resolve the issue of irregular sine and cosine values, take average
of diagonal elements and assign the average values.