Updated stats utils and added diagonal matrix