Fixing some matrix related calculations