Robust kidney stone tracking for a non-invasive ultrasound theragnostic system -servoing performance and safety enhancement-