Kalman filtering for ultrasound-based rigid registration in CAOS