Merge ukf into main branch 

Seems more stable and is definitely better than the ekf approach, so I'll make it the only approach.