Added distance and orientation averaging methods. Some more testing