optimized trilaterationV2 -- pls check for logic errors