Added fuction for compuating matrix T for which Tq(:) = U q U^T (:)